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ABSTRACT 


A  pure  pursuit  guidance  law  is  combined  with  a  heading 
autopilot  to  provide  accurate  path  keeping  of  submersible 
vehicles.  The  scheme  is  implemented  and  analyzed  in  both  the 
horizontal  and  vertical  planes.  A  complete  stability  analysis 
is  performed  in  order  to  evaluate  regions  of  stable  vehicle 
operations.  Numerical  integrations  support  the  analytic 
predictions.  Two  distinct  stability  boundaries  are 
established.  In  the  first,  the  vehicle  loss  of  stability  is 
accompanied  by  the  generation  of  oscillatory  motions  around 
the  commanded  path.  In  the  second,  loss  of  stability  occurs 
with  linearly  increasing  path  deviation.  The  horizontal  and 
vertical  plane  schemes  are  combined  with  a  propulsion  control 
law  in  order  to  achieve  path  tracking  of  a  general  commanded 
route  composed  of  several  straight  line  segments  in  three 
dimensional  space. 
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I  INTRODUCTION 


One  of  the  most  significant  functions  of  an  underwater 
vehicle  is  accurate  path  control  for  transiting  along 
prescribed  routes  in  three  dimensional  space.  The  commanded 
path  is  usually  described  by  a  series  of  way  points  in  space 
and  time  either  by  the  commander  or  by  a  path  planner  function 
in  the  case  of  an  unmanned  vehicle.  Without  significant  loss 
of  generality  we  can  assume  that  the  commanded  path  can  be 
approximated  by  straight  line  segments  between  consecutive  way 
points.  This  assumption  does  not  alter  the  important  features 
of  the  path  keeping  problem  since  every  smooth  path  can  be 
approximated  arbitrarily  closely  by  a  series  of  straight  line 
segments.  Once  a  desired  straight  line  path  has  been 
generated,  the  vehicle  guidance  and  autopilot  functions  are 
called  upon  to  ensure  satisfactory  path  keeping  through  the 
use  of  the  vehicle  actuators. 

One  way  to  ensure  that  the  vehicle  goes  through  a 
specified  sequence  of  way  points  is  by  using  a  heading 
autopilot  coupled  with  a  line  of  sight  guidance  scheme  [1]. 
The  scheme  proved  to  be  robust  enough  so  that  when  coupled 
with  an  independently  developed  depth  autopilot  [2],  accurate 
depth  control  was  maintained  while  transiting  between  way 
points  in  the  horizontal  plane.  The  disadvantage  associated 
with  this  technique  is  that  the  actual  vehicle  path  between 
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two  consecutive  way  points  differ  significantly  from  the 
corresponding  straight  line  segment. 

In  order  to  overcome  this  problem  and  achieve  accurate 
path  control  in  the  presence  of  obstacles  and  underwater 
currents,  a  cross  track  error  autopilot  was  developed  for  the 
horizontal  [3]  as  well  as  the  combined  horizontal  and  vertical 
planes  [4].  A  cross  track  error  autopilot  incorporates  the 
deviation  of  the  assumed  straight  line  path  into  the  control 
law  design.  This  requires  the  introduction  of  additional 
kinematic  relations  in  the  control  design  and,  as  a  result, 
the  controller  tends  to  be  more  sensitive  to  actual  system  / 
mathematical  model  mismatch. 

The  main  drawback  of  a  cross  track  error  autopilot  is  that 
it  represents  a  combined  guidance  /  control  scheme  with  no 
clear  distinction  between  these  two  functions.  Thus  it  is  very 
vehicle  specific  and  offers  little  flexibility  in  the  design. 
Path  control  is  limited  to  cross  track  error  only  and  analysis 
of  alternate  schemes  [5]  is  not  possible  unless  the  combined 
scheme  is  redesigned.  For  this  reason  we  decide  to  separate 
once  more  the  guidance  and  autopilot  functions  of  the  vehicle. 
An  orientation  controller  is  designed  in  order  to  provide 
accurate  vehicle  headings  in  response  to  guidance  commands. 
The  controller  is,  thus,  based  on  the  vehicle  dynamical 
equations  and  Euler  angle  rates.  A  guidance  scheme  is  used  to 
provide  appropriate  heading  commands  through  the  kinematic 
equations  of  inertial  position  rates.  A  line  of  sight  guidance 
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command  law  is  employed  as  in  [6]  and  [7].  We  consider  a 
reference  point  that  is  moving  ahead  of  the  vehicle  at  a 
constant  distance  on  the  desired  straight  line  path.  We  refer 
to  this  distance  as  the  lookahead  distance.  The  commanded 
heading  is  then  equal  to  the  line  of  sight  angle  between  the 
center  of  the  vehicle  and  the  lookahead  point.  By  suitably 
selecting  the  lookahead  distance  the  degree  of  convergence  of 
the  guidance  law  can  be  varied  from  very  slow  to  very  rapid 
onto  the  straight  line  path. 

Although  the  above  scheme  appears  to  be  trouble  free  on 
the  surface,  a  significant  complication  arises  in  the  case  of 
underwater  vehicles.  Since  the  actual  vehicle  response  is 
relatively  slow  as  dominated  by  the  existence  of  important 
dynamical  lags  there  is  the  possibility  of  instability  when 
the  guidance  and  control  functions  are  combined.  High  values 
of  the  lookahead  distance  result  in  very  slow  vehicle 
response.  The  problem  is  then  to  evaluate  these  regions  of 
stable  and  unstable  vehicle  response.  Chapter  II  of  this 
thesis  summarizes  the  stability  analysis  results  for  the 
horizontal  plane.  In  Chapter  III  we  proceed  with  the  analysis 
of  motions  under  the  guidance  and  control  scheme  for  the 
vertical  plane.  It  is  shown  that  the  existence  of  hydrostatic 
restoring  moments  here  due  to  the  nonzero  (positive) 
metacentric  height  brings  in  an  additional  form  of  instability 
not  present  in  the  horizontal  plane.  Finally  in  Chapter  IV  the 
previous  two  guidance  and  control  schemes  for  the  horizontal 
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and  vertical  planes  are  combined  and  with  a  speed  autopilot, 
accurate  path  tracking  in  three  dimensional  space  is  achieved. 
The  main  conclusion  of  this  work  is  that  guidance  and  control 
laws  for  underwater  vehicles  must  be  designed  together  even  if 
they  are  kept  separated,  in  order  to  ensure  stable  and 
satisfactory  path  keeping.  All  computations  in  this  work  are 
performed  for  the  Swimmer  Delivery  Vehicle  [8]  for  which  a 
complete  set  of  hydrodynamic  coefficients  and  geometric 
properties  is  available. 
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II.  HORIZONTAL  PLANE 


In  this  section  the  vehicle  equations  of  motion  for  the 
horizontal  plane  (x,y),  the  design  of  a  heading  autopilot  and 
simulations  and  stability  results  are  presented. 

A.  EQUATIONS  OF  MOTION 

For  the  horizontal  plane  the  mathematical  model  consists 
of  the  nonlinear  sway  and  yaw  differential  equations  shown 
below: 

m(  v+ur+x^r-ygr^)  =y  (2.1) 

I,r+mxQ{v^ur)  -my^ur^N  (2.2) 

Equations  (2.1), (2.2)  can  be  easily  derived  from  the  general 
six  degrees  of  freedom  equations  for  a  vehicle  by  assuming  all 
terms  off  the  horizontal  plane  to  be  zero.  The  equations  for 
the  sway  force  Y  and  yaw  moment  N  are  presented  below: 

Y=Y^r^{Y^v+Y^ur)  +y^uv--£  f  [Cph(^}  + 

2J  \v+ir\ 

N=N^Y+  {N^v+N  ur)  +Ar  f  [C^  h($)  g] 

I  V  z  V  2j  y  v+c,i 
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To  complete  the  model,  expressions  of  the  inertial 
position  rates  and  yaw  rate  are  required.  These  are  the 


kinematic  equations: 


tjf =r 

(2.3) 

x=ucosi|f- vsin!|; 

(2.4) 

y = u  s  i  in|;  -  VC  o  s  i|/ 

(2.5) 

B .  CONTROL  LAW 

It  is  more  convenient  for  the  design  of  a  linear  state 
space  heading  controller  to  represent  the  above  equations 
(2.1), (2.2), (2.3)  in  the  following  form  (with  7^=0): 

4r=r  (2.6) 

■C'=a^^uv+a^2^r+b^u^6+dyiv,  r)  (2.7) 

r=a2iUv+a22Ut+jb2U^6+<i^  ( V,  r)  (2.8) 

where : 

D={I^-N^)  im-Y^)  -  {mX(.-Y^) 

3x1  =  ^  I  Y,-{mx^-Y,)Nj 


6 


-921-5 1 

<922  =  -5  I  (777- y^)  {-mx^+N^)  -{mx^-N^)  (-m+Y^)  ] 
bi  =  ^^(I.-N,)Y,~(mx^~Y,)N,] 

^2  =  -5  [  (rn-Y^)  Y^-  {mxQ-N,^  Y^] 
djv,  r)  :.-llpC^^[(i^-M.)  I,^Y,I,] 

d^iv.  r)  =-l^pC^^[{m-Y^) 

Ij^=f  [h{^)  (v+ir)  1  (v+^r)  |]  di 
X2=/ [hil)  (v+5r)  |(v+5r)  W  d^ 

The  nonlinear  terms  d^(v,r)  ,d^(u,r)  are  small  and  can  be 
neglected  for  control  law  design.  They  are  kept,  however,  in 
all  numerical  simulations  that  follow. 

1.  ZERO  TAW  ANGLE 

When  the  commanded  yaw  angle  of  the  vehicle  is  zero  the 
control  law  has  the  following  form; 
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b=k^}^+k2V+kjr 


(2.9) 


where  are  computed  so  the  system  will  have  the  desired 
dynamics.  The  closed  loop  characteristic  equation  has  the 
following  form: 

A.^+aiX^+a2A,+a3=0  (2.10) 


where : 

a^ = a  3  u + a22  u +i>i  u  ^  i:2  +  jb2  u  ^  iCj 

^2  =(  ^11^22 +^1 A  “^3  322^^2 -^12^21 -^21-^1  “^3 

a^=  u^k^ 

The  characteristic  equation  is  specified  in  the  following 
way.  It  can  be  chosen  to  satisfy  the  minimum  ITAE  criterion 
where  it  assumes  the  form: 

A.-+a3X^+a2X+a3=0  (2.11) 


where : 


0^=1 .75(i)o 
a2=2 . 15<i)o 
a3=Wo 
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(t) 


c 


lOu 

tyl 


and  represents  the  dimensionless  settling  time  for  the 
system.  Equating  the  coefficients  of  equation  (2.10)  with  the 
desired  equation  (2.11)  and  after  some  algebra  we  find: 

2 -  (2.12) 

{b^a22-b2a^2^  (^2*311  "^1^21)  (2-13) 

k2b^u^  +k^b2u'^  =  -0.1-  (311+322)  u  (2.14) 

Selecting  a  value  for  t^  according  to  the  ITAE  criterion, 
dictates  complex  conjugate  dominant  poles  with  oscillatory 
transient  response.  It  was  found  that  other  poles  selections 
(for  example  real  negative)  do  not  change  significantly  the 
nature  of  the  results  and  the  stability  boundaries  that  are 
presented  later. 

2.  NOH  ZERO  TAW  ANGLE 

If  the  commanded  yaw  angle  is  non  zero  and  equal  to  i|r  j. 
then  the  control  law  (2.9)  is  simply  modified  to: 

b=k^(}^-^^)  +k2V+k^r  (2.15) 
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No  feedforward  term  is  necessary  in  (2.15)  since  no  rudder 
angle  is  required  to  keep  the  vehicle  to  a  constant  non  zero 
heading  angle  at  steady  state. 

C .  GUIDANCE 

The  heading  autopilot  that  was  designed  in  the  previous 
section  is  called  upon  now  to  provide  vehicle  path  in  the 
sense  of  passing  through  a  series  of  way  points  in  the 
horizontal  plane.  In  order  to  achieve  it  without  changing  the 
previously  designed  heading  autopilot  we  have  to  couple  it 
with  a  suitable  navigation  scheme  such  as  line  of  sight 
guidance . 

The  simplest  such  guidance  law  is  a  pure  pursuit 
navigation  which  is  accomplished  as  follows.  The  autopilot 
attempts  to  point  the  longitudinal  axis  of  the  vehicle  towards 
a  point  D  which  is  located  ahead  to  the  vehicle  on  the  nominal 
straight  line  path  at  a  fixed  distance  as  shown  in  Figure 
1.  This  target  distance  x^^  to  as  the  visibility,  lookahead,  or 
preview  distance.  The  line  of  sight  angle  a  is  defined  by: 

tana='-^  (2.16) 

Pure  pursuit  navigation  then  corresponds  to  taking: 

4r„=o  (2.17) 
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as  the  commanded  heading  angle  in  the  control  law  (2.15). 

It  can  be  seen  now  that  the  commanded  vehicle  heading 
angle  is  not  constant  but  it  is  function  of  the  vehicle 
position  y.  This  introduces  the  lateral  deviation  equation 

(2.5)  into  the  problem,  and  since  the  control  law  was  based  on 
equations  (2.6), (2.7)  and  (2.8)  only,  stability  of  the 
combined  autopilot-guidance  scheme  is  no  longer  guaranteed. 
Therefore,  we  need  to  develop  conditions  which  will  guarantee 
stability  and  ensure  satisfactory  path  keeping. 

D.  STABILITY 

The  complete  system  is  given  by  the  differential  equations 

( 2 . 6 )  , ( 2 . 7 ) , ( 2 . 8 ) ,  the  control  law  (2.15),  and  the  guidance 
equations  ( 2 . 16 ) , ( 2 . 17 ) .  The  trivial  equilibrium  state 
corresponding  to  a  straight  line  motion  is  characterized  by: 

\|r  =  v=r=y=0 


Linearization  of  the  state  equations  gives  the  following 
linear  system: 

X=AX 


where  the  complete  state  vector  is; 

X=  [\|»,  V,  r.y] 
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Local  stability  properties  are  established  by  the  eigenvalues 
of  [A]  The  characteristic  equation  is  found  to  be: 


+SA.3 + CA.2 +I5A. +E=  0 


(2.18) 


where : 


A=1 


C=-D^^B^C^~C^B.-A^ 

D=  -  C2  -  uD^ -A^B2  ■^A2B^ 


and 


A^=b^u'‘^k^ 

A2=b2u'^k^ 

B^=a^^u+b^u‘^k2 

B2=a2^u+b2u^k2 

C^=a^2^+b^u^k2 

C2=a22U*b2U^k^ 


D^=b^u^k^^ 
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D2=b2U^K-— 

■^d 


Loss  of  stability  occurs  when: 

BCD-B^E-AD^=0  (2.19) 

Equation  (2.19)  is  derived  from  Rooth’s  criterion  for  (2.18), 
and  it  corresponds  to  a  pair  of  complex  conjugate  roots 
crossing  the  imaginary  axis.  After  some  algebra  equation 
(2.19)  is  simplified  to: 

(2.20) 


where : 


•92  = 


(a^a2-2a3) 

b2^\\~b\^2i 


^3  = 


_  (-^1^22  -^2^12  -^2^  (-^1^22  •^2*^12  ■fc>2)u](X3 

(b2aii~bia2i)^u 


The  positive  root  of  equation  (2.20)  determines  the 
critical  value  of  x^  for  stability.  For  every  x^^  >  x^  critical 
system  is  stable  which  means  that  the  vehicle  will  follow  the 
path.  In  the  opposite  case  where  x^  <  x^  critical  system 
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becomes  unstable  and  the  motion  of  the  vehicle  becomes 
oscillatory  as  a  result  of  a  complex  conjugate  pair  of 
eigenvalues  with  positive  real  parts. 

Results  for  the  dimensionless  critical  visibility  versus 
settling  time  t^  are  presented  in  Figure  2.  These  results  are 
independent  of  the  forward  speed  since  gains  k^,k2,k3  are 
functions  of  u.  It  can  be  seen  from  Figure  2  that  for  higher 
t^  (softer  controller)  higher  lookahead  distance  is  required 
in  order  for  the  system  to  remain  stable.  It  is  obvious  that 
very  high  values  of  x^^  correspond  to  a  very  slow  navigator 
with  a  loss  in  speed  of  response  and  navigational  accuracy. 
The  results  of  this  section  establish  analytically  the  minimum 
required  lookahead  distance  that  is  required  for  stability 
based  on  linear  approximations. 

It  should  be  mentioned  that  all  results  in  this  work  are 
presented  in  dimensionless  form  unless  otherwise  mentioned. 
Nondimensionalizations  are  performed  by  using  the  vehicle 
length  and  the  vehicle  forward  speed. 
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Figure  2.  Regions  of  stability  in  the  horizontal  plane 
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E.  SIMULATIONS 


Numerical  simulations  confirm  the  results  of  the  stability 

analysis  of  Figure  2.  The  simulated  lateral  distance  y  (in 
vehicle  lengths)  versus  time  t  (in  dimensionless  seconds)  is 
shown  in  Figure  3  for  two  cases.  The  nominal  straight  line 
path  is  y=0  Case  1  is  located  in  Region  1  of  Figure  2  and  it 
can  be  seen  that  the  vehicle  response  is  unstable.  Case  2 
corresponds  to  a  stable  (t^,Xj)  combination  and  the  vehicle 
converges  to  the  desired  path. 
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Figure  3.  Stable  and  unstable  numerical  simulations 
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III.  VERTICAL  PLANE 


In  this  section  the  vehicle  equations  of  motion  for  the 
vertical  plane  (x,z),  the  design  of  a  vertical  heading 
autopilot  and  simulations  and  stability  results  are  presented. 

A.  EQUATIONS  OF  MOTION 

Restricting  our  attention  to  the  vertical  plane  the 
mathematical  model  consists  of  the  nonlinear  heave  and  pitch 
differential  equations  shown  below: 

/  •  •  2N  ^  (3-1) 

lyQ-wxQ  ( w-  uq)  +mzQwq=M  (3.2) 


where  only  vertical  plane  related  terras  have  been  kept.  The 
heave  force  Z  and  pitch  moment  M  are  written  as: 


Z=Z^q+  ( Z^w+Z  uq)  +Zjjlw-  -^fcpb(x)  -I— cix+  ( W-B)  cosB- 
^  w  g  *'  2  J  I  w-xq\ 

uHz,b,^z,b^) 


M=M^q-*-  {M^w-^MgUq)  +M^uw+-^  f  Cj^bix)  ^ ^  xdx-  (x^W-x^B)  cos6 

2  J 


-{ZqW-ZbB)  sinS  +  u^ 


In  the  above  equations  is  the  vehicle  weight,  B  the  buoyancy, 
(X5,Zg)  the  coordinates  of  the  center  of  gravity,  and  (Xg,Zg) 
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the  coordinates  of  the  center  of  buoyancy.  Also,  provision  for 
two  sets  of  control  surfaces  (stern  and  bow  planes)  is  made. 
The  kinematic  equations  are: 


x=ucos6+wsin6 

(3.3) 

z=-usin6+wcos0 

(3.4) 

0=g 

(3.5) 

B.  CONTROL  LAW 

The  linearized  state  space  form  of  equations  (3.1), (3.2) 
and  (3.5)  is  used  for  vertical  plane  heading  control: 

w=a^^uw+a^2^g+a^^Q+b^^u'^b  (3.6) 

g=a2iuv+a22ug+a230+b2iu265+b22u26^  (3.7) 

e=g 


where : 


D^=(!n-z^)  -(mxQ*Z^) 


^12  =  -^  ( im+z^)  +{niXs+Z^)  {Mg-m)  ] 

^\r 
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i3ii  =  ^  [  ily-M^)  2*^+  irnx^*Z^)  Wfi J 

^12  =  -^  f  <  -^y-^4^  ^6*“"  WftJ 

^21  =  -^  t  {m-ZjM^^  (mx^^M^)  ZJ 


322  =  -^  [  (m-Z^)  (M^-m)  +  (nvc^+Mj  im+Z^)  ] 


^23“  ^ 

^21  =  -^  f  im-Z^)M^^^  imx^^M^)  Z^,] 
^22  =  -^  [  (n7-ZjMj^+  (inx^+w^)  ZjJ 

Vy 


In  these  W=B  and  XQ=Xg  have  been  assumed.  Considering  that  the 
effect  of  the  bow  and  the  stern  planes  is  the  same  we  have: 


6,=6 

6^=-6 
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so 


^2~^21~^22 


From  the  above  the  final  form  of  the  equations  of  motion 
is: 

6=g 


w=a^.^uw+a^2^q+a^;^d+b^u^b 

(3.8) 

g=  a2 1  u  w+ 322  <32 3 ®  ^  ^ 

(3.9) 

1.  ZERO  PITCH  ANGLE 

When  the  commanded  direction  of  the  underwater  vehicle  is 
horizontal  the  control  law  has  the  following  form: 

b=k^Q+k2W+k^g  (3.10) 


where  k^,k2/k3  are  calculated  below.  From  the  system  of  the 
three  differential  equations  (3.5) , (3.8) , (3.9)  the  closed  loop 
characteristic  equation  has  the  following  form: 

\^+a^X^+a2k+a^=0  (3.11) 


where : 
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a^  =  -a^^u-b^u^k2~a22ii-b2u^kj 

a2=a^-ya22U.~ +a^.^b2U^ k^+a22biU‘-k2-a.^2^2\''^~ 

a2=a^^a2-^u-a-^^b2U^k2-b^a2^u^k^^a^ya2^u+a^^b2U^k-^+a2^b^u^k2 
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The  desired  characteristic  polynomial  according  to  the  ITAE 
criterion  is: 


X^+aiX^-t-a2A,+a3=0 


(3.12) 


where: 


a3=l  .75(i)o 
a2=2 . 15o)o 


“o 


lOU 

t^l 


and  ty  represents  the  dimensionless  settling  time  for  the 
vertical  plane  autopilot.  Equating  the  coefficients  of 
equation  (3.11)  with  equation  (3.12)  we  get: 

bj^u^k2+b2u'^k^  =  -a^-  ia.^^+a22)  u  (3.13) 


(^1^22 --^2312)  u-ic2+  (i>23ii-i:>ia2i)  u-k^^a^^b^u-k^ 
+a,,+  (a,,ap,-a,ia,2) 


(3.14) 


(■^2^11  ■^1^21 )  U^iC3+  (  a22^^~  ^12^2^ 
0:3+  (333a23-a33a23)  u 


(3.15) 


To  simplify  notations,  equations  ( 3 . 13 ) , ( 3 . 14 )  and  (3.15)  are 


written  as: 
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where : 


A2=b^u^ 

A3  =2^2 
Bl=jb2U^ 

•®2~  (■^l‘^22~'^2^12^ 

B^  =  (^2*^11  “^1^21^ 

^1~  (•^2^11~-^1^21  ^ 

^2~  ^ ‘^23-^1  ~^13'^2^ 

Z?i  =  -a3-(-aii+a22)  u 

•^2~®2''’^23''’ ^^12^2l“^ll'^22) 

^3  =“3+ (313^21-311323)  U 


From  the  above  system  of  equations  (3.16), (3.17), (3.18)  we 
can  find  3xpressions  for  the  gains  k^,k2,k3 


(3.19) 


_  ^2^2 

1”  c 

'■'1 


Aj  B^  Cj  +  Cj  -63^2  "  <^1^3  B2 


(3.20) 


(3.21) 


2.  NON  ZERO  PITCH  ANGLE 

When  the  commanded  pitch  angle  of  the  vehicle  is  not  equal 
to  zero,  we  have: 

Q=ay*e'  (3.22) 

a^  is  the  commanded  pitch  angle 
0 '  is  the  deviation  from  the  commanded  angle 

sin0=sina^,cos0'+cosa,,sin0'=sina^,-^0'cosa^. 

for  small  deviations  0'.  The  system  of  equations  of  motion 
(3.5), (3.8), (3.9)  takes  the  form: 

0^=qr  (3.23) 

w=a.^^uw+a^2^g+a^^cosa^d'+b.^u^b  +a^^sina^  (3.24) 


where : 


Then 
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g=a2^uw+a22uq+a2^cosa^Q' +h2U^b  +a23Sina^,  (3.25) 

The  control  law  now  takes  the  form: 

b=k^{Q-a^)  +k2W+k^q+k^  (3.26) 

where  k^jkj/kj  can  be  calculated  with  the  some  procedure  as 
before,  and  the  feedforward  gain  is  calculated  from  the 
desired  steady  state  accuracy.  At  steady  state  we  have: 

g=0 

0=a^, 

0^=0 

so  that  the  system  of  the  equations  of  motion 

(3.23) , (3.24) , (3.25)  yields: 

aj^^uw+jb;LU^6+aj3sina^=0  (3.27) 

a2iUw+b2U^b+a2jSinay=0  (3.28) 

Equations  ( 3 . 27 ) , ( 3 . 28 )  can  be  solved  for  the  steady  state 
values  of  6  and  w,  and  by  substitution  into  equation  (3.26), 
after  some  calculations  k^  is  found  to  be: 
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(3.29) 


k  -  ^13  (■33,->-.b2uic3)  -3^3  ia^^+b.uk^) 


Note  that  if  ay=0  or  2g=2g  then  k^=0. 

C.  GUIDANCE  LAW 

A  similar  to  the  horizontal  plane  case  guidance  law  can  be 
used  here  to  allow  path  keeping  in  the  vertical  plane.  To  the 
previous  system  of  differential  equations  (3.1), (3.2), (3.5) 
one  more  equation  is  added,  the  kinematic  equation  (3.4).  The 
new  system  is  now  going  to  be  examined  for  two  different 
cases . 

a)  Horizontal  path  (no  change  in  depth) 

b)  Inclined  path  (change  in  depth) 

1.  HORIZONTAL  PATH 

In  this  case  where  the  commanded  depth  remains  the  same 
the  control  law  is: 

b=k^(B-Q^)  +k2W+k,g  (3.30) 

where  0^  is  the  commanded  line  of  sight  (pitch  angle) 

e^=tan-^—  (3.31) 
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where  k, ,k2,k3  are  already  known  from  the  previous  section,  and 
is  the  visibility  distance  similar  to  the  horizontal  case, 
shown  in  Figure  4. 

2 .  INCLINED  PATH 

Here  the  commanded  depth  changes  linearly  so  that  the 
angle  6  is  given  by: 

b=k.^{Q-a^-Q'  +k-w-^k^q*k^  (3.32) 


where  k^,k2,k3,k^  are  the  same  as  previously  determined. 

The  k^  term  exists  here  because  an  angle  6*0  has  to  remain 
when  the  underwater  vehicle  changes  depth  to  equalize  the 
restoring  moment  due  to  the  pitch  angle.  The  commanded  pitch 
angle  is: 

0^'=tan-^-^  (3.33) 

where  z'  is  the  cross  track  error  off  the  inclined  path  as 
shown  in  Figure  5 . 

D.  STABILITY 

The  complete  system  is  given  by  the  equations  of  motion 
(3.1),  (3.2),  (3.4),  (3.5),  the  control  law  (3.30),  and  the 
guidance  law  (3.31).  Horizontal  motion  at  the  commanded  depth 
is  characterized  by: 

d=w=g=z=0 
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Linearization  of  the  above  equations  produces  the  linear 
system: 

X=AX 


where : 


X=  [0,  w,  g,  z] 


and 


0 

0 

1 

0 

A  = 

-  K  u  ^  — - 

^d 

^23  ^GB  ^  -^1 

-u 

1 

0 

0 

where; 

(3.35) 

is  the  metacentric  height.  Stability  properties  of  the 
straight  line  motion  are  established  by  the  eigenvalues  of 
matrix  [A] .  It  should  be  mentioned  that  from  now  until  the  end 
of  this  chapter  a, 3,  a^j  have  been  redefined  to  show  explicitly 
the  metacentric  height  Z^g. 

A  program  is  written  to  compute  the  eigenvalues  of  matrix 
(3.34)  over  a  range  of  (t^,Xj)  values,  and  detect  whether  one 


32 


or  more  eigenvalues  become  unstable.  Typical  results  are  shown 
in  Figure  6  for  u=5  ft/sec  and  2gg=0.1. 

1.  REGIONS  OF  STABILITY 

It  can  be  seen  that  the  stability  boundary  of  Figure  6 
separates  the  parameter  space  into  three  regions: 

1:  Unstable  region,  one  pair  of  complex  conjugate 
eigenvalues  of  [A]  has  positive  real  parts. 

2:  Stable  region,  all  eigevalues  of  [A]  have  negative 
real  parts. 

3:  Unstable  region,  one  real  positive  eigenvalue  of 

[A]. 

Obviously,  stable  vehicle  response  is  not  possible  unless  the 
parameters  (x^,ty)  are  chosen  in  region  2. 
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1:  One  pair  of  complex  conjugate  eigevalues  with 
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2.  SIMULATIONS 


Before  proceeding  further  with  the  stability  analysis, 
numerical  integrations  are  first  performed  in  order  to  examine 
the  response  of  the  vehicle  in  each  of  the  above  three  regions 
of  stability  of  Figure  6.  The  same  parameters  u=5  ft/sec  and 
2gg=0.1  ft  are  used.  Simulations  for  the  pitch  angle  0  and  the 
commanded  line  of  sight  angle  0^  for  the  case  ty=5  and  x^j=4  are 
shown  in  Figure  7.  This  corresponds  to  region  2  of  Figure  6 
which  is  the  region  of  stability.  The  simulation  results  show 
that  the  actual  vehicle  pitch  angle  approaches  the  commanded 
angle,  after  some  oscillations,  and  the  depth  reaches  its 
commanded  value  at  zero  as  predicted. 

When  the  visibility  distance  is  x^=0.4  with  the  same  t^, 
the  vehicle  moves  into  the  unstable  region  1  of  Figure  6.  The 
simulated  response  is  shown  in  Figure  8  where  oscillatory 
characteristics  are  exhibited.  If  we  keep  the  same  value  for 
x^j=4  and  we  change  the  controller  time  constant  t^=15  we  enter 
the  unstable  region  3  of  Figure  6.  The  simulated  vehicle 
response  is  shown  in  Figure  9  where  it  appears  that  0  and  0^. 
diverge  and  they  both  reach  nonzero  steady  state  values.  As  a 
result  the  vehicle  depth  is  now  a  linear  function  of  time, 
without  ever  stabilizing.  These  results  require  a  more 
detailed  analysis  of  the  regions  of  stability  of  the 
controller  /  guidance  combination. 
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ZcB  =  0.1  (ft) 
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Figure  9.  Numerical  simulation  in  region  3 
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E.  ANALYSIS 


The  stability  properties  of  the  system  are  characterized 
by  the  eigenvalues  of  the  linearized  matrix  [A],  given  by 
equation  (3.34).  The  characteristic  equation  of  [A]  has  the 
form: 

AX^+BA.^+CX2+DA,+S=0  (3.36) 


where : 


A=1 


S=a^ 

C=a2+i?iu2ic,A 

£>=013+ 

^d  ^d 

^~^GB  ^^2^13~^1^23^  ■*■  ^■^2^11  "■^1 '^21^ 

^d  ^d 

According  to  Routh's  criterion  (3.36)  has  one  pair  of  complex 
conjugate  roots  crossing  the  imaginary  axis  when: 

BCD-AD^-B^E=0  (3.37) 

After  some  algebra  ,  equation  (3.37)  can  be  written  as  : 
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a3(ai«2-«3)-?^d+[c?i(aia2-“3)  +a^c^a2-a^d^-alej  (3.39) 

Xjj+di  (ttiC^'di)  =0 


where: 

Ci=A2-^1 
d^  =  {-B2-A^u) 
ei=  (-C2+C^u) 

and  A2,A3,B2  C^,C2  were  defined  previously  following  equations 
(3.16) , (3. 17)  and  (3.18). 

The  positive  root  of  equation  (3.38)  provides  the  critical 
value  of  Xjj  for  stability.  This  produces  the  curve  separating 
the  regions  1  and  2  of  Figure  6.  As  the  (x^j^t^)  combinations 
cross  into  region  1,  the  response  of  the  system  becomes 
oscillatory  as  a  result  of  the  pair  of  complex  conjugate 
eigenvalues  with  positive  real  part.  This  explains  the 
simulations  observed  in  Figures  7  and  8. 

A  different  kind  of  instability  occurs  when  one  real  root 
of  (3.36)  crosses  zero.  For  this  to  happen  the  condition  is: 

E=Q  (3.39) 

and  using  the  previous  definition  of  E,  this  happens  when 
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(3.40) 


k^=0 

Equations  (3.40)  and  (3.19)  yield 


Equations  ( 3 . 41 ) , ( 3 . 20 )  define  then  the  critical  condition  for 
stability.  In  our  case  this  can  be  simplified  as  follows.  The 
expression  for  C2  is  reproduced  here. 

<^2=  (^23^1-^13^2)  (3.42) 

Since  we  have  assumed  that  bow  and  stern  planes  have  the  same 
strength 

and  substituting  the  expressions  for  ajj , b^ ,  a^3 , b2  we  can  find 
that 

C2=0  (3.43) 

Equations  (3.43)  and  (3.41)  then  require  that 
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(3.44) 


D^=0 

and  using  the  definition  for  Dj  we  get 

a,+  (  <913^21 

or 

(  )  ■*■  (^13^21 '^11^23  ^ 

and  we  can  find  the  critical  value  of  t^  as 

f.  ^ _ lOu _ 

''czicical  JL  (3.45) 

1  [  (  ^11^23  ■^13'32i)  ^ 

Condition  (3.45)  shows  that  the  critical  value  of  t^  is 
independent  of  which  is  demonstrated  in  Figure  6  as  a 
straight  line  parallel  to  the  axis.  Furthermore,  the  other 
stability  curve,  equation  (3.38),  intersects  the  t^  axis  at 
Xj=0  when  k^=0  which  is  the  same  condition  as  (3.45).  This 
means  that  the  stability  conditions  (3.38)  and  (3.45)  separate 
the  (X|j,t^)  parameter  space  into  three  regions  of  stability, 
as  shown  in  Figure  6 . 

Results  of  the  stability  regions  for  Zgg=0  are  shown  in 
Figure  10.  These  are  independent  of  the  forward  speed  u  just 
as  in  the  horizontal  plane  case.  It  should  be  mentioned  that 
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for  Zgg=0,  ^  00  and  therefore  , region  3  of  figure  6 
never  appears . 

For  Zgg  >  0,  the  stability  regions  depend  heavily  on  the 
forward  speed  u.  This  is  demonstrated  in  Figure  11  for  Zgg=0.1 
(ft)  and  various  values  of  u  in  (ft/sec).  As  the  speed  is 
decreased  the  critical  value  of  t^  from  (3.45)  also  decreases 
with  the  effect  of  reducing  region  1  and  enlarging  region  2 
and  3 . 

The  effect  of  varying  the  metacentric  height  Zgg  while 
keeping  u  constant  is  evaluated  in  Figure  12  for  u=2.  Similar 
conclusions  can  be  drawn  for  this  case  as  previously. 

The  critical  value  of  t^  as  given  by  (3.45)  is  shown  in 
Figure  13  for  different  values  of  the  forward  speed  u  and  the 
metacentric  height  z^g.  The  surface  shown  in  the  figure 
separates  the  stability  regions  2  and  3. 

The  final  task  of  this  section  is  to  explain  the 
simulations  observed  in  Figure  9  when  the  vehicle  operates  in 
region  3. 
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Figuro  12 .  Regions  of  stability  for  u  *  2  ft/sec 
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F.  STEADY  STATE  SOLUTIONS 


It  was  shown  in  the  previous  section  that  transition 
between  Regions  2  and  3  is  associated  with  a  real  eigenvalue 
crossing  zero.  Usually  when  such  a  loss  of  stability  occurs 
and  the  primary  equilibrium  solution  becomes  unstable, 
additional  stable  equilibrium  solutions  appear.  To  evaluate 
these  new  steady  state  solutions  we  consider  the  complete 
system  given  by  equations  (3.4),  (3.5),  (3.6),  (3.7),  (3.30), 

and  (3.31).  At  steady  state  the  time  derivatives  vanish  and  we 
get 

g=0  (3.46) 

w=-^sine  (3.47) 

6=:^Iflsine  (3.48) 


Substituting  equations  ( 3 . 46 ) , ( 3 . 47 ) ,  and  (3.48)  into  equation 
(3.4)  we  get : 

C, 

( -u+— pcos0)  sin0=O  (3.49) 

W 

Equation  (3.49)  may  accept  besides  the  normal  solution  6=0, 
another  solution  given  by: 
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(3.50) 


COS0  =  — u=- 


(•^1^23  ■^2^13)^05 


Equation  (3.50)  is  valid  provided  cos©  <  1  which  means: 


(-^1^23  -^2^13)  ■^GB 
■^2^11 --^1^21 


(3.51) 


If  (3.51)  is  satisfied  the  equilibrium  angle  0  can  be 
determined  from  (3.50)  provided: 

^3_?2sine^a^3t  (3-52) 

^1 

where  6^,^  is  the  maximum  dive  plane  angle  typically  set  at 
0 . 4  radians . 

In  our  case  conditions  (3.51)  and  (3.52)  are  not 
satisfied,  which  means  that  the  non  zero  equilibrium  pitch 
angle  cannot  be  computed  from  (3.50).  Furthermore  z  0  at 

steady  state,  which  means  that  5=constant.  Therefore,  z  is 
linearly  increasing  with  time,  and 

tan'^— as.  ..  t-o®  (3.53) 

2 
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Substituting  equations  (3.46),  (3.47),  (3.48),  and  (3.53)  into 
the  control  law  (3.30)  and  (3.31)  we  can  find  the  equation  for 
the  unknown  steady  state  pitch  angle. 

sin0=ic^Ci  (0--J )  +iCjC2sin0  (3.54) 

If  we  call  0  -  n/2  =0',  equation  (3.54)  reduces  to: 

iCj^Ci0'+ (aj-iCjCj)  cos0'=O  (3.55) 

It  can  now  be  seen  that  equation  (3.55)  has  a  solution  when  k^ 
crosses  zero  which  is  the  same  condition  for  transition 
between  regions  (2)  and  (3)  found  in  the  previous  section.  The 
steady  state  solution  is  then  computed  from  (3.55)  if: 

sin0^6^^,  (3.56) 


or  from: 


sin0  = 


^3-«3 


sat 


(3.57) 


otherwise. 

Results  for  the  steady  values  of  0  and  6  are  presented  in 
Figures  14  and  15  versus  for  u=5  and  ty=15. 
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Solid  lines  correspond  to  stable  and  dashed  lines  to  unstable 
equilibrium  positions.  It  can  be  seen  that  the  simulation 
results  for  Zgg  =0.1  of  Figure  9  are  verified. 

The  steady  state  pitch  angle  0=0  loses  its  stability  at 
Zgg=0.07  and  begins  to  increase  together  with  the  dive  plane 
angle  6.  This  is  up  to  Zgg=0.12  where  6  reaches  its  maximum 
value.  For  increasing  Zgg  beyond  this  point,  the  pitch  angle 
6  begins  to  decrease  since  6  remains  constant.  These  results 
are  for  fixed  t^  and  u.  Results  for  different  values  of  the 
controller  settling  time  t^  and  vehicle  speed  u  are  shown  in 
Figures  16  and  17  respectively. 
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(degrees) 


u  =  5  (ft/sec) 


Figure  14.  Steady  state  pitch  angle  6  versus  Zga 


52 


5  (degrees) 

10.0  20.0  30.0 


Figure  15.  Steady  state  dive  plane  angle  5  versus  Zqb 
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IV  THREE  DIMENSIONAL  GUIDANCE  CONTROL 


The  horizontal  and  vertical  plane  guidance  and  control 
laws  that  were  developed  in  the  previous  two  chapters  are 
combined  here  to  provide  accurate  path  keeping  in  three 
dimensional  space.  The  other  requirement  is  that  the  forward 
speed  along  the  path  should  be  constant  and  equal  to  a 
commanded  value.  This  will  enable  path  tracking  instead  of 
simply  path  keeping. 

A.  PROPULSION  CONTROL 

Just  as  the  horizontal  (vertical)  plane  path  control 
design  was  based  on  the  linearized  lateral  (vertical) 
equations  of  motion,  the  propulsion  control  law  will  be  based 
on  the  linearized  form  of  the  surge  equation  only.  The  surge 
equation  is; 


where; 


a  = 


Hnax 


(4.2) 


n  is  the  propeller  revolutions,  and  6  the  dive  plane  angle. 
Only  w  and  6  terms  remain  in  equation  (4.1)  because  only  these 
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terms  are  nonzero  at  steady  state  for  a  constant  commanded 
dive  or  rise  angle.  A  propulsion  control  law  is  introduced  of 
the  form: 

n=no+k^(u-u^)  (4.3) 

The  feedback  gain  is  computed  from  stability 

reguirements  whereas  the  feedforward  term  n^  is  computed  from 
steady  state  accuracy.  When  n=nQ  the  forward  speed  u  must 
equal  the  commanded  speed  u^.  Therefore,  (4.1)  becomes: 

f(u^)^C^a'^n^=0  (4.4) 

where  we  defined 

f(u)  +  (4.5) 


The  terms  w  and  6  are  given  as  functions  of  u^.  and  the 
commanded  pitch  angle  a^  by 


(4.6) 


(4.7) 


Solving  (4.4)  for  ng  we  get 
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(4.8) 


n 


2 

0 


f(Ue) 


This  term  guarantees  the  required  steady  state  accuracy.  To 
evaluate  we  substitute  (4.3)  and  (4.8)  into  (4.1)  and  we 
get : 

(m-Xj)  u-2C^^a^r!oic„(u-u^)  =0  (4.9) 


The  characteristic  equation  of  (4.9)  is 


m-x,s 


(4.10) 


The  desired  characteristic  equation  is 


s+Wo=0,  .  .o)o=-^ 


(4.11) 


where  t^  is  the  desired  dimensionless  settling  time  for  the 
speed  control.  Comparing  (4.10)  with  (4.11)  we  can  solve  for 
the  control  gain. 


5u^{in-X^ 
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With  the  choice  of  gains  (4.12)  and  (4.8),  the  propulsion 
control  law  (4.3)  is  complete. 


B.  THREE  LIMEMSIONAL  PATH  KEEPING 

Suppose  the  commanded  path  is  a  general  straight  line  in 
three  dimensions,  from  point  O  to  point  F  as  shown  in  Figure 
18.  The  vehicle  position  is  at  point  A.  With  respect  to  the 
inertial  coordinate  frame  (x,y,2)  the  commanded  path  is 
characterized  with  the  two  angles  and  a^  as  shown  in  the 
Figure.  In  order  to  achieve  the  commanded  path,  a  coordinate 
frame  rotation  by  an  angle  a„  is  performed  first  as  shown  in 
Figure  19.  The  necessary  geometric  relations  are: 


a;,=tan'^ 


Xp-x^ 


(4.13) 


x'=  iy-Vo'^  sina^.+  {x-xj  cosa^  (4.14) 

y '=  (y-yo)  cosa^-  {x-x^)  sina^  (4.15) 


The  rudder  control  law  is  then  of  the  form: 

b=k^{f^-aff-a„)  +k2V+k-^r  (4.16) 


where  the  line  of  sight  angle  for  horizontal  plane  control 
is  defined  by: 
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(4.17) 


tano 


is  the  lookahead  distance  determined  according  to  the 
stability  analysis  of  Chapter  II,  and  k^,  kj,  kj  are  the 
horizontal  plane  control  gains  from  Chapter  II. 

Another  rotation  by  an  angle  a^  is  conducted  next  as  shown 
in  Figure  20.  The  geometric  relations  here  are: 


ay=tan~^  ^ 

(4.18) 

x'p 

■=  (y^-yo)  sina^+  Up-x^)  cosa^ 

(4.19) 

x"=-  iz-z^)  sina^+x'cosay 

(4.20) 

z'-  iz-z^)  cosa,,+x'sina^. 

(4.21) 

The  dive  plane  control  law  is: 

6=k^(Q-a^-a^) +k2W+kjg+k^  (4.22) 

where  the  line  of  sight  angle  for  vertical  plane  control  is 
defined  by: 

tano  =-^ 
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Xjjy  is  the  lookahead  distance  determined  according  to  the 
stability  analysis  of  Chapter  III,  and  k^,  k^,  kj,  k^  are  the 
vertical  plane  control  gains  as  computed  in  Chapter  III.  The 
existence  of  two  distinct  distances  x^^^,  x^^  is  for  maximum 
flexibility  in  the  design  and  to  allow  for  the  possibly 
different  stability  conditions  for  horizontal  and  vertical 
plane,  as  analyzed  in  the  previous  two  chapters. 

Results  are  presented  for  a  typical  three  dimensional 
commanded  route  that  consists  of  the  following  way  points  (x, 
y,  z)  =  (20,  0,  5),  (40,  5,  5),  (60,  -5,  -3),  (100,  0,  -5) 
vehicle  lengths  with  individual  straight  line  paths  connecting 
them.  Switch  from  one  to  the  next  straight  line  path  was 
initiated  when  the  vehicle  position,  measured  along  the 
current  commanded  path,  was  within  a  specified  target  distance 
(TD)  from  the  way  point.  Parameters  used  for  the  simulation 
were  the  following:  t^=7,  t^=5,  Zg=0.1,  t^=0.2,  Xj,j^=3,  Xjy=2.5 
commanded  speeds  u=(4,  4,  5,  5)  for  the  four  straight  line 
segments  respectively,  and  TD=1.  Simulation  results  are 
presented  in  Figure  21  through  25.  It  can  be  seen  from  Figures 
21  that  accurate  path  control  is  maintained  in  both  the 
horizontal  and  vertical  planes.  Speed  control  is  also  very 
accurate,  see  Figure  22,  despite  the  course  changes  and 
nonzero  dive  and  rise  angles.  The  speed  controller  revolutions 
per  minute  are  shown  in  Figure  23,  where  the  maximum 
saturation  limit  is  set  500  rpm.  Rudder  response  is  shown  in 
Figure  25  where  the  steady  state  nonzero  values  occur  during 
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a  nonzero  conunanded  pitch  angle.  Comparing  Figures  24  and  25 
with  22,  it  can  be  observed  that  the  vehicle  slows  down 
momentarily  when  the  control  surfaces  become  active,  a 
situation  which  is  quickly  corrected  by  the  speed  controller. 
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Figure  20.  Vertical  plane  rotation 


Figure  21.  Numerical  simulation  for  3-D  path  keeping 


66 


in  1 

[  I  I  i  "  I  '  I  I  I  " '  I  I  [  ■  i  I  i  I  I  I 

0.  0  100.  0  200.  0  300.  0 

t 


Figure  22 .  Time  history  of  vehicle  speed  u 


400.  0 
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Figure  23.  Time  history  of  propeller  revolutions  per  minute 
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Figure  25.  Time  history  of  dive  plane  angle 
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CONCLUSIONS  AND  RECOMMENDATIONS 


The  main  conclusions  and  contributions  of  this  work  can  be 
summarized  as  follows: 

1.  Pursuit  guidance  law  and  decoupled  horizontal  and 
vertical  plane  orientation  controllers  were  shown  to  provide 
accurate  vehicle  path  keeping  in  three  dimensions. 

2.  The  scheme  proved  to  be  robust  enough  so  that  it  could 
handle  the  nonlinear  coupling  between  speed  response,  and 
horizontal  and  vertical  plane  motions  without  performance 
degradation . 

3.  It  was  shown  that  the  guidance  and  control  schemes  must 
be  designed  together  in  order  to  avoid  loss  of  stability  or 
excessive  oscillatory  response. 

4.  Analytic  conditions  for  stability  were  derived.  The 
conditions  were  expressed  explicitly  in  terms  of  the  vehicle 
hydrodynamic  characteristics  and  the  guidance  and  control  law 
design  specifications. 

5.  An  extensive  study  of  the  mechanism  of  loss  of 
stability  was  undertaken  for  the  vertical  plane  motions.  Two 
distinct  possibilities  were  discovered  and  analyzed.  In  the 
first  one  pair  of  complex  conjugate  eigenvalues  crosses  the 
imaginary  axis  and  results  in  an  oscillatory  vehicle  behavior 
around  the  commanded  path.  In  the  second  ,  one  real  eigenvalue 
crosses  zero  and  the  vehicle  drifts  off  to  a  steady  state  path 
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with  its  deviation  from  the  commanded  path  linearly  increasi^^^g  ' 
with  time.  This  new  path  was  computed  and  explicit  conditions 
to  avoid  such  a  undesirable  situation  were  given. 

Some  recommendations  for  further  research  include  the 
following: 

1.  Comparisons  from  the  point  of  view  of  path  keeping 
response  under  physical  system  /  mathematical  model  mismatch. 

2.  State  estimators  must  be  included  in  the  analysis  to 
evaluate  performance  under  partial  state  knowledge  and  sensor 
noise . 
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APPENDIX  A 


PROGRAM  SIM_3D.F0R 

FOTIS  A.  PAPOULIAS/ANGELLOS  G.  PAPASOTIRIOU 
NAVAL  POSTGRADUATE  SCHOOL 
AUGUST  1991 

VEHICLE  THREE  DIMENSIONAL  PATH  KEEPING 
HEADING  AUTOPILOTS 
PURE  PURSUIT  NAVIGATION 

SIMULTANEOUS  RUDDER/DIVE  PLANE  SWITCHINGS 
DECLARATIONS 

REAL  L,MASS, IX, lY, IZ, IXZ, lYZ, IXY 
REAL  KIH , K2H , K3H , KIV, K2V, K3V, K4V, KN 

RE^ L  KPDOT , KRDOT , KPQ , KQR , KVDOT , KP , KR , KVQ , KWP , KWR , KV , KVW , 
&  KPN,KDB 

REAL  MQDOT , MPP , MPR , MRR , MWDOT , MQ , MVP , MVR , MW , MW , 

&  MDS , MDB , NDRB 

REAL  NPDOT , NRDOT , NPQ , NQR , NVDOT , NP , NR , NVQ , NWP , NWR , 

&  NV,NVW,NDRS 

REAL  MM(6,6),INDX(100) 

DIMENSION  X(9)  ,BR(9),HH(9),VECH1(9)  ,VECH2(9)  ,XM1'^INV(6,6) 
DIMENSION  VECV1(9) ,VECV2(9) ,F(12) ,FP(6) ,DISV(100) 
DIMENSION  XDES(IOO) ,YDES(100) ,ZDES(100) ,UDES(100) , 

Sc  DISH(IOO) 


GEOMETRIC  PROPERTIES 

WEIGHT 

'=12000.0 

IX 

=  1760.0 

lY 

=  9450.0 

IZ 

=10700.0 

IXY 

=  0.0 

lYZ 

=  0.0 

IXZ 

=  0.0 

L 

=  17.425 

RHO 

=  1.94 

G 

=  32.2 

XG 

=  0.0 

YG 

=  0.0 

XB 

=  0.0 

YB 

=  0.0 

ZB 

=  0.0 
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AO  =  2.0 

CDO  =  0.0057 

MASS  =WEIGHT/G 
BOY  =WEIGHT 
RPMMAX=  500.0 
RPMMIN=  -500.0 
UMAX  =  6.0 

UMIN  =  0.1 

ALPHA  =UMAX/RPMMAX 

C 

C  SURGE  HYDRODYNAMIC  COEFFICIENTS 

C 

XPP 
XQQ 
XRR 
XPR 
XUDOT 
XWQ 
XVP 
XVR 
XQDS 
XQDB 
XRDR 
XW 
XWW 
XVDR 
XWDS 
XWDB 
XDSDS 
XDBDB 
XDRDR 
XRES 
XPROP 

C 

C  SWAY  HYDRODYNAMIC  COEFFICIENTS 

C 

YPDOT  =  1.270E-04*0.5*RHO*L**4 
YRDOT  =  1.240E-03*0.5*RHO*L**4 
YPQ  =  4.125E-03*0.5*RHO*L**4 
YQR  =-6.510E-03*0.5*RHO*L**4 
YVDOT  =-5.550E-02*0.5*RHO*L**3 
YP  =  3.055E-03*0.5*RHO*L**3 
YR  =  2.970E-02*0.5*RHO*L**3 
YVQ  =  2 . 360E-02*0.5*RHO*L**3 
YWP  =  2.350E-01*0.5*RHO*L**3 
YWR  =-l .880E-02*0.5*RHO*L**3 
YV  =-9.310E-02*0.5*RHO*L**2 
YVW  =  6.840E-02*0.5*RHO*L**2 
YDRS  =+2.270E-02*0.5*RHO*L**2 
YDRB  =+2.270E-02*0.5*RHO*L**2 

C 


=  7.030E-03*0.5*RHO*L**4 
=-l .470E-02*0.5*RHO*L**4 
=  4.010E-03*0.5*RHO*L**4 
=  7.640E-04*0.5*RHO*L**4 
=-7.580E-03*0.5*RHO*L**3 
=-l . 920E-01*0 .5*RHO*L**3 
=-3.240E-03*0.5*RHO*L**3 
=  1.890E-02*0.5*RHO*L**3 
=  2.610E-02*0.5*RHO*L**3 
=-2 . 600E-03*0.5*RHO*L**3 
=-8.180E-04*0.5*RHO*L**3 
=  5.290E-02*0.5*RHO*L**2 
=  1.710E-01*0.5*RHO*L**2 
=  1.730E-03*0.5*RHO*L**2 
=  4.600E-02*0.5*RHO*L**2 
=  9.660E-03*0.5*RHO*L**2 
=-1.160E-02*0.5*RHO*L**2 
=-8.070E-03*0.5*RHO*L**2 
=-1.010E-02*0.5*RHO*L**2 
=  CD0*0.5*RHO*L**2 
=  XRES*ALPHA**2 
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HEAVE  HYDRODYNAMIC  COEFFICIENTS 

ZQDOT  =-6.810E-03*0.5*RHO*L**4 
ZPP  =  1 . 270E-04*0 . 5*RHO*L**4 
ZPR  =  6.670E-03*0.5*RHO*L**4 
ZRR  =-7.350E-03*0.5*RHO*L**4 
ZWDOT  =-2.430E-01*0.5*RHO*L**3 
ZQ  =-l  ■?50E-01*0 .5*RHO*L**3 
ZVP  =-4.810£-02*0.5*RHO*L**3 
ZVR  =  4.550E-02*0.5*RHO*L**3 
ZW  --3.020E-01*0.5*RHO*L**2 
ZW  =-6.840E-02*0.5*RHO*L**2 
ZDS  =-2.270E-02*0.5*RHO*L**2 
ZDB  =-2 .270E-02*0.5*RHO*L**2 

ROLL  HYDRODYNAMIC  COEFFICIENTS 

KPDOT  =-l . 010E-03*0 . 5*RHO*L**5 
KRDOT  =-3.370E-05*0.5*RHO*L**5 
KPQ  =-6.930E-05*0.5*RHO*L**5 
KQR  =  1 . 680E-02*0 .5*RHO*L**5 
KVDOT  =  1.270E-04*0.5*RHO*L**4 
KP  =-l . 100E-02*0.6*RHO*L**4 
KR  =-8 . 410E-04*0 . 5*RHO*L**4 
KVQ  =~5. 115E-03*0.5*RHO*L**4 
KWP  =-l .270E-04*0.5*RHO*L**4 
KWR  =  1.390E-02*0.5*RHO*L**4 
KV  =  3.055E-03*0.5*RHO*L**3 
KVW  =-1.870E-01*0.5*RHO*L**3 

PITCH  HYDRODYNAMIC  COEFFICIENTS 

MQDOT  =-1.680E-02*0.5*RHO*L**5 
MPP  =  5.260E-05*0.5*RHO*L**5 
MPR  =  5.040E-03*0.5*RHO*L**5 
MRR  =-2.860E-03*0.5*RHO*L**5 
MWDOT  =-6.810E-02*0.5*RHO*L**4 
MQ  =-6.860E-02*0.5*RHO*L**4 
MVP  =  1 . 180E-03*0.5*RHO*L**4 
MVR  =  I .730E-02*0.5*RHO*L**4 
MW  =  9.860E-02*0.5*RHO*L**3 
MW  =-2.510E-02*0.5*RHO*L**3 
MDS  =-l . 113E-02*0 .5*RHO*L**3 
MDB  =  1.113E-02*0.5*RHO*L**3 

YAW  HYDRODYNAMIC  COEFFICIENTS 

NPDOT  =-3.370E-05*0.5*RHO*L**5 
NRDOT  =-3.400E-03*0.5*RHO*L**5 
NPQ  =-2. 110E-02*0.5*RHO*L**5 
NQR  =  2 .750E-03*0.5*RHO*L**6 
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NVDOT  =  1 .240E-03*0.5*RHO*L**4 
NP  =-8.405E-04*0.5*RHO*L**4 

NR  =-1.640E-02*0.5*RHO*L**4 

N\7Q  =-9 . 990E-03*0 . 5*RHO*L**4 
NWP  =-1.750E-02*0.5*RHO*L**4 
NWR  =  7.350E-03*0.5*RHO*L**4 
NV  =-7 .420E-03*0.5*RHO*L**3 
NVW  =-2 . 670E-02*0.5*RHO*L**3 
NDRS  =-1.113E-02*0.5*RHO*L**3 
NDRB  =+l . 113E-02*0.5*RHO*L**3 

OPEN  DATA  AND  RESULTS  FILES 

OPEN  ( 10, FILE=’ PATH_3D.DAT’ , STATUS= ’ OLD ’ ) 
OPEN  (11,FILE='XY.RES’ ,STATUS=‘NEW' ) 

OPEN  ( 12,FILE= 'XZ .RES ■ ,STATUS='NEW' ) 

OPEN  (13, FILE= • DRS . RES ’ , STATUS= ' NEW ’ ) 

OPEN  (14,FILE=’DS.RES' , STATUS= ' NEK ' ) 

OPEN  (15,FILE=' YCTE.RES* , STATUS= ' NEW ' ) 
OPEN  (16,FILE='ZCTE.RES’ , STATUS= ‘ NEW ' ) 
OPEN  (17,FILE='XYZ.RES' , STATUS= ' NEW ' ) 

OPEN  ( 18,FILE= 'U.RES ' , STATUS= ' NEW ' ) 

OPEN  (19,FILE='RPM.RES' , STATUS= ' NEW ’ ) 

OPEN  (20,FILE= ' PHI .RES ' , STATUS= ' NEW ' ) 

OPEN  ( 2 1,FILE=' THETA. RES' , STATUS= ' NEW ' ) 
OPEN  (22,FILE='PSI.RES' , STATUS= ’ NEW ' ) 

OPEN  (23,FILE='V.RES' , STATUS= ' NEW ' ) 

OPEN  (24,FILE='R.RES' , STATUS= ' NEW ' ) 

OPEN  (25,FILE='W.RES' , STATUS= ' NEW ’ ) 

OPEN  (26,FILE='Q.RES' , STATUS= ' NEW ' ) 

OPEN  (27,FILE='YZ.RES* , STATUS= *  NEW  * ) 

READ  DATA  FILE 

READ  (10,*)  TSIM,DELTA, IPRNT 
READ  (10,*)  I PTS, TARGET 
READ  (10,*)  TN,TH,TV,ZG 
IF  ( IPTS.GT. 100)  IPTS=100 
DO  1  I=1,IPTS 

READ  (10,*)  XD, YD,ZD,XDH,XDV,U0 
XDES( I )=XD*L 
YDES( I )=YD*L 
ZDES(I)=ZD*L 
UDES( I)=U0 
DISH( I )=XDH*L 
DISV( I )=XDV*L 
1  CONTINUE 

MASS  MATRIX  INITIALIZATION  AND  DEFINITION 


DO  15  J=l,6 
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DO  10  K=l,6 

XMMINV( J,K)=0.0 
MM( J,K)=0.0 

10  CONTINUE 
15  CONTINUE 

C 

MM(1,1)=  MASS-XUDOT 
MM(1,5)=  MASS*ZG 
MM(1,6)=-MASS*YG 
C 

MM(2,2)=  MASS-YVDOT 
MM( 2 , 4 ) =-MASS*ZG-YPDOT 
MM(2,6)=  MASS*XG-YRDOT 
C 

MM(3,3)=  MASS-ZWDOT 
MM(3,4)=  MASS*YG 
MM( 3 , 5 ) =-MASS*XG-ZQDOT 
C 

MM(4,2)=-MASS*ZG-KVDOT 
MM(4,3)=  MASS*YG 
MM (4,4)=  IX-KPDOT 
MM(4,5)=-IXY 
MM(4,6)=-IXZ-KRDOT 
C 

MM(6,1)=  MASS*ZG 
MM( 5 , 3 ) =-MASS*XG-MWDOT 
MM(5,4)=-IXY 
MM(5,5)=  lY-MQDOT 
MM(5,6)=-IYZ 
C 

MM(6, 1)=-MASS*YG 
MM(6,2)=  MASS*XG-NVDOT 
MM(6,4)=-IXZ-NPDOT 
MM(6,5)=-IYZ 
MM(6,6)=  IZ-NRDOT 
C 

C  MASS  MATRIX  INVERSION 

C 

DO  12  1=1,6 
DO  11  J=l,6 

XMMINV(I, J)=0.0 

11  CONTINUE 
XMMINV( I, I )=1 . 0 

12  CONTINUE 

CALL  INVTA(MM, 6, INDX,D) 

DO  13  J=l,6 

CALL  INVTB(MM,6, INDX,XMMINV(1, J) ) 

13  CONTINUE 
C 

C  VARIABLES  INITIALIZATION 

C 


77 


PISIM  =TSIM/DELTA 
ISIM  =PISIM 
ECHO  =1.0 /DELTA 
lECHO  =ECHO 
YAW  =0 . 0 
SWAY  =0 . 0 
PITCH  =0.0 
HEAVE  =0.0 
U  =UDES(1) 

RPM  =UDES( 1 ) /ALPHA 
V  =0.0 

W  =0.0 

P  =0.0 

Q  =0.0 
R  =0.0 
DS  =0.0 
DB  =0.0 
DR  =0.0 

TWOPI  =8.0*ATAN(1.0) 

PI  =0.5*TWOPI 
PHI  =0.0 
ISTART=1 
TARGET=TARGET*L 
XPOS  =0.0 
YPOS  =0.0 
ZPOS  =0.0 
CDY  =0.5 
CDZ  =0.5 
JPRNT  =0 
UK  =0 
JE  =0 
DRS  =0.0 
DRB  =0.0 
DS  =0.0 
DB  =0.0 

C 

C  DEFINE  THE  LENGTH  X,  BREADTH  BR,  AND  HEIGHT  HH  TERMS 

C 

X(l)  =  -105.9/12.0 

X(2)  =  -99.3/12.0 

X(3)  =  -87.3/12.0 

X(4)  =  -66.3/12.0 

X(5)  =  72.7/12.0 

X(6)  =  83.2/12.0 

X(7)  =  91.2/12.0 

X(8)  =  99.2/12.0 

X(9)  =  103.2/12.0 

C 

HH(1)  =  0.00/12.0 

HH(2)  =  8.24/12.0 

HH(3)  =  19.76/12.0 
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HH(4) 

29.36/12.0 

HH(5) 

= 

31.85/12.0 

HH(6) 

= 

27.84/12.0 

HH(7  ) 

= 

21.44/12.0 

HH(8) 

= 

12.00/12.0 

HH(9) 

= 

0.00/12.0 

BR(1) 

= 

0.00/12.0 

BR(2) 

= 

8.24/12.0 

BR(3) 

= 

19.76/12.0 

BR(4) 

= 

29.36/12.0 

BR(5) 

= 

31.85/12.0 

BR(6) 

= 

27.84/12.0 

BR(7) 

= 

21.44/12.0 

BR(8) 

= 

12.00/12.0 

BR(9) 

= 

0.00/12.0 

AUXILLIARY  VARIABLES  FOR  HORIZONTAL  PLANE  CONTROL 

DH  =( IZ-NRDOT)*(MASS-YVDOT)- 
&  (MASS*XG-YRDOT)*(MASS*XG-NVDOT) 

A1 1H= ( ( I Z -NRDOT ) * YV- ( MASS*XG-yRDOT ) *NV ) /DH 
A12H=( (IZ-NRDOT)*(-MASS+YR)- 
&  (MASS*XG-YRDOT)* (-MASS*XG+NR) ) /DH 

A2 1H= ( ( MASS- YVDOT ) *NV- (MASS*XG-NVDOT ) * YV ) /DH 
A22H=( (MASS -YVDOT) *( -MASS* XG+NR) - 
&  (MASS*XG-NVDOT)*(-MASS+YR) )/DH 

B1 1H= ( < IZ-NRDOT) *YDRS- (MASS*XG-yRDOT) *NDRS ) /DH 
B12H= ( (IZ-NRDOT ) * YDRB- (MASS*XG-YRDOT ) *NDRB ) /DH 
B21H=( (MASS-YVDOT)*NDRS-(MASS*XG-NVDOT)*YDRS)/DH 
B22H*((MASS- YVDOT ) * NDRB -(MASS  *XG- NVDOT ) *  YDRB ) / DH 
BIH  =B11H-B12H 
B2H  =B21H-B22H 

AUXILLIARY  VARIABLES  FOR  VERTICAL  PLANE  CONTROL 

DV  = (MASS-ZWDOT) * ( I Y-MQDOT) -ZQDOT*MWDOT 
A1 1 V= ( ( I Y-MQDOT ) * ZW+ZQDOT*MW ) /DV 
A12V= ( ( I Y-MQDOT ) * ( ZQ+MASS ) +ZQDOT*MQ ) /DV 
A13V=- ( ZG-ZB ) * (MASS*XG+ZQDOT) *WEIGHT/DV 
A2 1V= ( MWDOT*  ZW+ ( MASS-ZWDOT ) *MW ) /DV 
A22V= (MWDOT* ( ZQ+MASS )+ (MASS-ZWDOT) *MQ) /DV 
A23V=- ( ZG-ZB ) * (MASS-ZWDOT) *WEIGHT/DV 
B11V=( (IY-MQDOT)*ZDS+ZQDOT*MDS)/DV 
B12V= ( ( I Y-MQDOT ) *ZDB+ZQDOT*MDB ) /DV 
B21V= (MWDOT*ZDS+ (MASS-ZWDOT) *MDS ) /DV 
B22V= (MWDOT*ZDB+ (MASS-ZWDOT )*MDB)/DV 
BIV  =B11V-B12V 
B2V  =B21V-B22V 
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SIMULATION  BEGINS 


LOOP  OVER  WAY  POINTS 

DO  200  IP=1,IPTS 

IF  (IP.GE.2)  GO  TO  210 
XDH=DISH( 1) 

XDV=DISV( 1 ) 

UO  =UDES(1) 

XD  =XDES(1) 

YD  =YDES(1) 

ZD  =ZDES(1) 

XD1=0.0 

YD1  =  0.0 

ZD1=0.0 

XD2=XD 

YD2=YD 

ZD2=ZD 

GO  TO  211 

210  XDH=DISH(IP) 

XDV=DISV( IP) 

UO  =UDES(IP) 

XD  =XDES(IP) 

YD  =YDES(IP) 

ZD  =ZDES(IP) 

XD1=XD2 

YD1=YD2 

ZD1=ZD2 

XD2=XD 

YD2=YD 

ZD2=ZD 

211  ZD12=ZD2-ZD1 
XD12=XD2-XD1 
YD12=YD2-YD1 

HORIZONTAL  HEADING  CONTROL  GAINS 

OMEGAH=(10.0*U0)/(TH*L) 

AD1H=1 . 75*OMEGAH 

AD2H=2 . 15*OMEGAH**2 

AD3H=OMEGAH**3 

A1=B1H*U0*U0 

B1=B2H*U0*U0 

C1=-AD1H-(A11H+A22H)*U0 

A2=(B1H*A22H-B2H*A12H)*U0**3 

B2=(B2H*A11H-B1H*A21H)*U0**3 

KlH=AD3H/( (B2H*A11H-B1H*A21H)*U0**3) 

C2=AD2H-(A11H*A22H-A12H*A21H)*U0**2+B2H*U0*U0*K1H 

K2H=(C1*B2-C2*B1)/(A1*B2-A2*B1) 

K3H=(C2*A1-C1*A2 )/(Al*B2-A2*Bl ) 
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VERTICAL  HEADING  CONTROL  GAINS 

OMEGAV= (10. 0  *U0 ) / ( TV*L ) 

AD1V=1 .75*OMEGAV 

AD2V=2. 15*OMEGAV**2 

AD3V=OMEGAV**3 

A2=B1V*U0*U0 

A3=B2V*U0*U0 

D1=-AD1V-(A11V+A22V)*U0 

B1=-B2V*U0*U0 

B2=(B1V*A22V-B2V*A12V)*U0**3 

B3=(B2V*A11V-B1V*A21V)*U0**3 

D2=AD2V+A2  3V+ ( A1 2V*A2 IV-Al IV* A2  2V ) *U0  *  *2 

C1=(B2V*A11V-B1V*A21V)*U0**3 

C2=(A23V*B1V-A13V*B2V)*U0**2 

D3=AD3V+ ( A1 3V*A2 IV-Al 1V*A23V ) *U0 

K2V=(A3*B1*D3+C1*B3*D1-D2*C1*A3) 

K2V=K2V/(A3*B1*C2+C1*B3*A2-C1*A3*b: ) 

K1V=(D3-C2*K2V)/C1 

K3V=(D1-A2*K2V)/A3 

ALPHAH=ATAN(YD12/XD12) 

ALPHAH=ABS ( ALPHAH ) 

IF  ( (XD12,GE.0.0) .AND. (YD12.GE.0.0) )  ALPHAH=  ALPHAH 
IF  ( (XD12.GE. 0.0) .AND. (YD12.lt. 0.0) )  ALPHAH=  -ALPHAH 
IF  ( (XD12.lt. 0.0) .AND. ( YD12 . GE . 0 . 0 ) ) ALPHAH=P I -ALPHAH 
IF  ( (XD12.lt. 0.0) .AND. (YD12.lt. 0.0) )  ALPHAH=PI+ALPHAH 
XCTEH= ( YPOS-YDl ) *SIN (ALPHAH )+ (XPOS-XDl ) * COS (ALPHAH ) 
YCTE  = ( YPOS-YDl ) *COS (ALPHAH ) - ( XPOS-XDl ) *  S IN ( ALPHAH ) 
XIP  =YD1 2  *  S IN ( ALPHAH ) +XD1 2  *  COS ( ALPHAH ) 
ALPHAV=ATAN(ZD12/X1P) 

ALPHAV=ABS ( ALPHAV ) 

IF  (ZD12.GE.0.0)  ALPHAV= -ALPHAV 

K4V=-(A13V*(A21V+B2V*U0*K2V)-A23V* (A11V+B1V*U0*K2V) ) 
K4V=K4V*SIN(ALPHAV) /( (B1V*A21V-B2V*A11V) *U0*U0 ) 

ZCTE  =  (ZPOS-ZDl )*COS(ALPHAV)+XCTEH*SIN(ALPHAV) 
XCTEV=-(ZPOS-ZDl)*SIN(ALPHAV)+XCTEH*COS (ALPHAV) 

PROPULSION  CONTROL  GAIN 

WSS=(B1V*A23V-B2V*A13V)*SIN( ALPHAV) 

WSS=WSS/ ( ( A1 1V*B2V-A21V*B1V) *U0 ) 
DSS=(A21V*A13V-A11V*A23V)*SIN( ALPHAV) 

DSS=DSS/ ( ( A1 1V*B2V-A21V*B1V) *U0*U0 ) 
FUC=XWW*WSS**2+U0*WSS*(XWDS-XWDB)*DSS 
&  +U0*U0*(XDSDS+XDBDB)*DSS**2-XRES*U0**2 

RPM0=-FUC/ ( XRES*ALPHA*  *2 ) 

RPM0=SQRT(RPM0) 

WRITE  (*,*)  RPM0,U0/ALPHA 

KN=-5.0*U0*(MASS -XUDOT ) / ( XRE  S  * ALPHA  * ALPHA  *  RPMO  *  TN  *  L ) 
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WRITE  (*,201)  XD/L,YD/L,ZD/L 

SIMULATION  FOR  EACH  WAY  POINT 

DO  100  I=ISTART, ISIM 
ICOUNT=I 

IF  (U.LT.UMIN)  U=UMIN 

CALCULATE  THE  DRAG  FORCE,  INTEGRATE  THE  DRAG  OVER 

THE  VEHICLE 


DO  600  K=l,9 

UCF=(V+X(K)*R)**2+(W-X(K)*Q)**2 

UCF=SQRT(UCF) 

IF  (UCF.LT. 1 .E-6)  GO  TO  601 
CFLOW=CDY*HH(K) * (V+X(K) *R) **2+CDZ*BR( K) * 
&  (W-X(K)*Q)**2 

VECH 1 ( K ) =CFLOW* ( V+X ( K ) *R ) /UCF 
VECH2 ( K ) =CFLOW* ( V+X ( K ) *R ) *X ( K ) /UCF 
VECVl(K)=CFLOW*(W-X(K)*Q)/UCF 
VECV2 ( K ) =CFLOW* ( W-X ( K ) *Q ) *X ( K ) /UCF 

600  CONTINUE 

CALL  TRAP ( 9, VECV1,X, HEAVE) 

CALL  TRAP ( 9, VECV2,X, PITCH) 

CALL  TRAP (9, VECH 1,X, SWAY  ) 

CALL  TRAP(9,VECH2,X,YAW  ) 

HEAVE=-0 . 5*RHO*HEAVE 
PITCH=+0 . 5*RHO*PITCH 
SWAY  =-0.5*RHO*SWAY 
YAW  =-0.5*RHO*YAW 
GO  TO  602 

601  HEAVE=0.0 
PITCH=0.0 
SWAY  =0.0 
YAW  =0 . 0 

602  CONTINUE 

FORCE  EQUATIONS 


SURGE  FORCE 


& 

& 

fit 

fit 

fit 

fit 

& 


FP(1)  =  MASS*V*R-MASS*W*Q+MASS*XG*Q**2+MASS*XG*R**2- 
MASS*YG*P*Q-MASS*ZG*P*R+XPP*P**2+XQQ* 
Q**2+XRR*R**2+XPR*P*R+XWQ*W*Q+XVP*V*P+ 
XVR*V*R+U*Q* (XQDS*DS+XQDB*DB ) + 
U*R*(XRDRS*DRS+XRDRB*DRB)+XVV*V**2+XWW* 
W**2+U*V*(XVDRS*DRS+XDRB*DRB)+U*W* 
(XWDS*DS+XWDB*DB)+(XDSDS*DS**2+XDBDB*DB**2+ 
XDRDR* ( DRS*  *  2+DRB*  *  2 ) ) *U*  *  2 - ( WEIGHT-BOY ) * 
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& 


SIN ( THETA ) +XPROP*RPM*RPM-XRES*U*U 


C 

C 

C 


C 

C 

C 


C 

C 

C 


C 

C 

c 


c 

c 

c 


c 


SWAY  FORCE 


FP(2)  =-MASS*U*R-MASS*XG*P*Q+MASS*YG*R**2-MASS*ZG*Q*R+ 
YPQ*P*Q+YQR*Q*R+YP*U*P+YR*U*R+YVQ*V*Q+ 
YWP*W*P+YWR*W*R+YV*U*V+YVW*V*W+YDRS*U**2*DRS+ 
YDRB*U*  *  2  *DRB+ ( WE IGHT-BOY ) * 

COS ( THETA) *SIN( PHI )+MASS*W*P+MASS*YG*P**2+SWAY 

HEAVE  FORCE 

FP(3)  =  MASS*U*Q-MASS*V*P-MASS*XG*P*R-MASS*YG*Q*R+ 
&  MASS*ZG*P**2+MASS*ZG*Q**2+ZPP*P**2+ 

&  ZPR*P*R+ZRR*R**2+ZQ* 

&  U*Q+ZVP*V*P+ZVR*V*R+ZW*U*W+ZW*V**2+HEAVE+ 

fit  U*  *2  * ( ZDS*DS+ZDB*DB )  +  ( WEIGHT-BOY ) * 

fit  COS ( THETA )*COS( PHI) 

ROLL  MOMENT 


fit 

fit 

fit 

fit 

fit 

fit 


fit 

fit 

fi^ 

fit 

fit 

fit 

& 

fit 


fit 

fit 

fit 

fi^ 

fit 

fit 


FP(4)  =  -IZ*Q*R+IY*Q*R-IXY*P*R+IYZ*Q**2- 

IYZ*R**2+IXZ*P*Q+MASS*YG*U*Q-MASS* 
YG*V*P-MASS*ZG*W*P+KPQ*P*Q+KQR*Q*R+ 
KP*U*P+KR*U*R+KVQ*V*Q+KWP*W*P+ 
KWR*W*R+KV*U*V+KVW*V*W+ ( YG*WEIGHT-YB*BOY ) * 
COS ( THETA ) *COS ( PHI ) - ( ZG*WEIGHT- 
ZB*BOY)*COS( THETA) *SIN(PHI ) +MASS*ZG*U*R 

PITCH  MOMENT 

FP(5)  =  -IX*P*R+IZ*P*R+IXY*Q*R-IYZ*P*Q- 
IXZ*P**2+IXZ*R**2-MASS*XG*U*Q+ 
MASS*XG*V*P+MASS*ZG*V*R- 
MASS*ZG*W*Q+MPP*P**2+ 

MPR*P*R+MRR*R**2+MQ* 
U*Q+MVP*V*P+MVR*V*R+MW*U*W+ 
MW*V**2+U**2*(MDS*DS+MDB*DB)-(XG*WEIGHT- 
XB*BOY)*COS(THETA)*COS(PHI)- 
(ZG*WEIGHT-ZB*BOY)*SIN( THETA )+PITCH 

YAW  MOMENT 

FP(6)  -  -IY*P*Q+IX*P*Q+IXY*P**2-IXY*Q**2+IYZ*P*R- 
IXZ*Q*R-MASS*XG*U*R+MASS*XG*W*P-MASS*YG* 
V*R+MASS*YG*W*Q+NPQ*P*Q+NQR*Q*R+NP*U*P+NR* 
U*R+NVQ*V*Q+NWP*W*P+NWR*W*R+NV*U*V+ 
NVW*V*W+NDRS*U**2*DRS+NDRB*U**2*DRB+ 
(XG*WEIGHT-XB*BOY)*COS(THETA)*SIN(PHI)+ 
(YG*WEIGHT-YB*BOY)*SIN (THETA )+YAW 
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COMPUTE  THE  RIGHT  HAND  SIDE  OF  XDOT=F(X) 

DO  610  J  =  1,6 
F(J)  =  0.0 
DO  611  K  =  1,6 

F(J)  =  XMMINV(J,K)*FP(K)  +  F(J) 

611  CONTINUE 

610  CONTINUE 

INERTIAL  POSITION  RATES 

F(7)  =  U*COS ( PSI ) *COS (THETA) +V*( COS (PSI)*SIN( THETA)* 
&  SIN(PHI)-SIN(PSI)*COS(PHI) ) +W* ( COS ( PSI ) * 

&  SIN ( THETA) *COS ( PHI )+SIN( PSI )*SIN( PHI) ) 

F(8)  =  U*SIN(PSI)*COS(THETA)+V*(SIN(PSI)*SIN(THETA)* 
&  SIN(PHI)+COS(PSI)*COS(PHT) ) +W* ( S IN ( PS  I ) * 

fit  SIN ( THETA) *COS( PHI) -COS ( PSI )*SIN( PHI ) ) 

F(9)  =  -U*SIN(THETA)+V*COS(THETA)*SIN(PHI)+ 

&  W*COS ( THETA )*COS( PHI) 

EULER  ANGLE  RATES 

F ( 10 ) =  P+Q*  SIN ( PHI ) *TAN ( THETA ) +R*COS ( PHI ) *TAN ( THETA ) 
F(ll)=  Q*COS(PHI)-R*SIN(PHI) 

F(12)=  Q*SIN(PHI)/COS(THETA)+R*COS( PHI) /COS (THETA) 

ASSIGN  XDOT  VECTOR 

UDOT  =  F ( 1 ) 

VDOT  =  F ( 2 ) 

WDOT  =  F ( 3 ) 

PDOT  =  F ( 4 ) 

QDOT  =  F ( 5 ) 

RDOT  =  F ( 6 ) 

XDOT  =  F ( 7 ) 

YDOT  =  F ( 8 ) 

ZDOT  =  F(9) 

PHIDOT  =  F(10) 

THEDOT  =  F(ll) 

PSIDOT  =  F(12) 

FIRST  ORDER  INTEGRATION 

U  =  U  +  DELTA*UDOT 

V  =  V  +  DELTA*VDOT 

W  =  W  +  DELTA*WDOT 
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P  =  P 

Q  =  Q 

R  =  R 


XPOS 

=  XPOS 

YPOS 

=  YPOS 

ZPOS 

=  ZPOS 

PHI 

=  PHI 

THETA 

=  THETA 

PSI 

=  PSI 

+  DELTA* PDOT 
+  DELTA *QDOT 
+  DELTA*RDOT 
+  DELTA*XDOT 
+  DELTA* YDOT 
+  DELTA* Z DOT 
+  DELTA*PHIDOT 
+  DELTA*THEDOT 
+  DELTA*PSIDOT 


VELOCITY  INPUT  CALCULATION 


UC=U0 

IF  (UC.GE.UMAX)  UC=UMAX 
IF  (UC.LE.UMIN)  UC=UMIN 

RPM  INPUT  CALCULATION 

RPMO=UC /ALPHA 
RPM=RPMO+KN*(U-UC) 

IF  ( RPM . GE . RPMMAX )  RPM=RPMMAX 
IF  (RPM.LE.RPMMIN)  RPM=RPMMIN 

COORDINATE  TRANSFORMATIONS 

XCTEH=  ( YPOS-YDl ) *SIN( ALPHAH ) + ( XPOS-XDl ) *COS ( ALPHAH ) 
YCTE  =  (YPOS-YDl)*COS(ALPHAH)-(XPOS-XDl)*SIN(ALPHAH) 
ZCTE  =  (ZP0S-ZD1)*C0S(ALPHAV)+XCTEH*SIN(ALPHAV) 
XCTEV=-(ZPOS-ZDl)*SIN(ALPHAV)+XCTEH*COS(ALPHAV) 

HIT  CRITERIA 

VTOTAL=(XD2-XDl )**2+(ZD2-ZDl)**2 
VTOTAL=SQRT ( VTOTAL ) 
HT0TAL=(XD2-XD1)**2+(YD2-YD1)**2 
HTOTAL=SQRT ( HTOTAL ) 

VAWAY  =VTOTAL-XCTEV 
VAWAY  =ABS( VAWAY) 

HAWAY  =HTOTAL-XCTEH 
HAWAY  =ABS( HAWAY) 

IF  ( ( VAWAY . LT . TARGET ) . OR . ( HAWAY . LT . TARGET ) )  GO  TO 
&  101 

DIVE  PLANE  INPUT  CALCULATION 
ZPHI=ZCTE 

S I G V= ATAN ( Z  PH I / XDV ) 

DS=K1V* ( THETA-ALPHAV-SIGV) +K2V*W+K3V*Q+K4V 

IF  (DS.GE.  0.4)  DS=  0.4 
IF  (DS.LE.-0.4)  DS=-0.4 
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C 


DB=-DS 


RUDDER  INPUT  CALCULATION 

YPHI=YCTE 

S IGH=-ATAN ( YPHI /XDH ) 

DRS=K1H*(PSI-ALPHAH-SIGH)+K2H*V+K3H*R 

IF  (DRS.GE.  0.4)  DRS=  0.4 
IF  (DRS.LE.-0.4)  DRS=-0.4 

DRB=-DRS 

PRINT  RESULTS 

TIME=I*DELTA 

JE=JE+1 

IF  ( JE.NE. lECHO)  GO  TO  99 
JE=0 

99  JPRNT=JPRNT+1 

IF  ( JPRNT.NE. IPRNT)  GO  TO  100 

IJK=IJK+1 

TIME=I*DELTA 

WRITE  (11,*)  XPOS/L,YPOS/L 

WRITE  (12,*)  XPOS/L,ZPOS/L 

WRITE  (13,*)  TIME, DRS*180. 0/PI 

WRITE  (14,*)  TIME, DS*180. 0/PI 

WRITE  (15,*)  TIME,YCTE/L 

WRITE  (16,*)  TIME,ZCTE/L 

WRITE  (17,*)  XPOS/L,YPOS/L,ZPOS/L 

WRITE  (18,*)  TIME,U 

WRITE  (19,*)  TIME,RPM 

WRITE  (20,*)  TIME, PHI*180. 0/PI 

WRITE  (21,*)  TIME, (THETA-ALPHAV) *180. 0/PI 

WRITE  (22,*)  TIME, (PSI-ALPHAH) *180. 0/PI 

WRITE  (23,*)  TIME,V 

WRITE  (24,*)  TIME,R 

WRITE  (25,*)  TIME,W 

WRITE  (26,*)  TIME,Q 

WRITE  (27,*)  YPOS/L,ZPOS/L 

JPRNT=0 

100  CONTINUE 
GO  TO  500 

101  ISTART=ICOUNT 

200  CONTINUE 
500  STOP 

201  FORMAT  (’  HEADING  FOR  (X,Y,Z)  =  (  ',F9.3,'  ,  ',F9.3,'  , 

&  ' ,F9 . 3  '  )  '  ) 

END 
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SUBROUTINE  TRAP ( N , A, B , OUT ) 

NUMERICAL  INTEGRATION  ROUTINE  USING  THE  TRAPEZOIDAL  RULE 

DIMENSION  A( 1 ) ,B( 1 ) 

N1=N-1 
OUT=0.0 
DO  1  1=1, N1 

OUT1=0.5*(A(I)+A(I+1) )*(B{I+1)-B(I) ) 

OUT  =OUT+OUTl 
1  CONTINUE 
RETURN 
END 


SUBROUTINE  INVTA(MM, N , INDX, D) 

PARAMETER  (NMAX=100 , TINY=1 . OE-20 ) 

DIMENSION  INDX(6)  ,W(NMAX) 

REAL  MM(6,6) 

D=1 

DO  12  1  =  1, N 
AAMAX=0. 

DO  11  J=1,N 

IF(ABS(MM( I, J) ) .GT.AAMAX)  AAMAX=ABS(MM( I , J) ) 

11  CONTINUE 

IF  (AAMAX.EQ.O. )  PAUSE  ' SINGULAR  MATRIX ' 

W(I)  =  1./AAMAX 

12  CONTINUE 

DO  19  J=1,N 
DO  14  1=1, J-1 
SUM=MM( I, J) 

DO  13  K=1,I-1 

SUM=SUM-MM ( I , K ) *MM ( K , J ) 

13  CONTINUE 
MM( I, J)=SUM 

14  CONTINUE 
AAMAX=0. 

DO  16  I=J,N 
SUM=MM( I, J) 

DO  15  K=1,J-1 

SUM=SUM-MM( I,K)*MM(K, J) 

15  CONTINUE 
MM( I, J)=SUM 
DUM=W(I)*ABS(SUM) 

IF  ( DUM . GE . AAMAX )  THEN 
IMAX=I 
AAMAX=DUM 
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END  IF 

16  CONTINUE 

IF  ( J.NE. IMAX)THEN 
DO  17  K-1,N 

DUM=MM( IMAX,K) 
MM(IMAX,K)=MM( J,K) 

MM( J,K)=DUM 

17  CONTINUE 
D=-D 

W(  IMAX)=W(  J) 

ENDIF 

INDX( J)=IMAX 

IF(MM( J, J) .EQ.O. )MM( J, J)=TINY 
IF( J.NE. N) THEN 
DUM=1 . /MM( J, J) 

DO  18  I=J+1,N 

MM(I, J)=MM(I, J)*DUM 

18  CONTINUE 
ENDIF 

19  CONTINUE 
RETURN 
END 


SUBROUTINE  INVTB (MM, N, INDX, B ) 
DIMENSION  INDX(N),B(N) 

REAL  MM(6,6) 

11  =  0. 

DO  12  1=1, N 
LL=INDX( I) 

SUM=B(LL) 

B(LL)=B( I ) 

IF  ( II .NE. 0)THEN 
DO  11  J=II,I-1 

SUM=SUM-MM ( I , J ) *B ( J ) 

11  CONTINUE 

ELSE  IF  (SUM.NE.O)  THEN 
II  =  I 
ENDIF 
B(I)=SUM 

12  CONTINUE 

DO  14  I=N,1,-1 
SUM=B( I ) 

IF  (I.LT.N)THEN 
DO  13  J=I+1,N 

SUM=SUM-MM( I, J)*B( J) 

13  CONTINUE 
ENDIF 

B(I)=SUM/MM(I,I) 

14  CONTINUE 
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RETURN 

END 
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APPENDIX  B 


PROGRAM  VERT_STAB . FOR 

REGIONS  OF  STABILITY  -  VERTICAL  PLANE 
PARAMETERS  ARE:  XD  AND  TV 
NUMERICAL  OR  ANALYTIC  COMPUTATION 

IT  NEEDS  FILE  " SUBRTNS . FOR"  OR  ANY  STANDARD  EIGENVALUE 

SOLVER 

IMPLICIT  DOUBLE  PRECISION  (A-H,0-Z) 

DOUBLE  PRECISION  KIV, K2V, K3V, L 

DOUBLE  PRECISION  MQDOT, MQ, MW, MWDOT , MDS , MDB , MASS , I Y 
DIMENSION  A(4,4) ,FV1(4),IV1(4) ,ZZZ(4,4) ,WR(4) ,WI(4) 

OPEN  (10,FILE=’BIF0.RES' , STATUS= ’ NEW' ) 

OPEN  (11,FILE='BIF1.RES’ , STATUS= ' NEW ’ ) 

OPEN  (12,FILE='BIF2.RES' , STATUS =' NEW ' ) 

OPEN  (13,FILE='BIF3.RES' , STATUS= ’ NEW ' ) 

WEIGHT=12000.0 
lY  =  9450.0 

L  =  17.425 

RHO  =  1.94 

G  =  32.2 

XG  =  0.0 

ZB  =  0.0 

MASS  =WEIGHT/G 
BOY  =WEIGHT 

ZQDOT  =-6.810E-03*0.5*RHO*L**4 
ZWDOT  =-2.430E-01*0.5*RHO*L**3 
ZQ  =-1.350E-01*0.5*RHO*L**3 
ZW  =-3.020E-01*0.5*RHO*L**2 
ZDS  =-2.270E-02*0.5*RHO*L*'*2 
ZDB  =-2 .270E-02*0.5*RHO*L**2 
MQDOT  =-1.680E-02*0.5*RHO*L**5 
MWDOT  =-6.810E-02*0.5*RHO*L**4 
MQ  =-6.860E-02*0.5*RHO*L**4 
MW  =  9.860E-02*0.5*RHO*L**3 
MDS  =-1.113E-02*0.5*RHO*L**3 
MDB  =  1 . 113E-02*0.5*RHO*L**3 

WRITE  (*,1001) 

READ  (*,*)  TVMIN,TVMAX, ITV 
WRITE  (*,1002) 

READ  (*,*)  XDMIN,XDMAX, IXD 
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XDMIN=XDMIN*L 
XDMAX=XDMAX*L 
WRITE  (*,1003) 

READ  (*,*)  U,ZG 

WRITE  (*,1004) 

READ  (*,*)  ISOL 

AUXILIARY  VARIABLES 

DV  = (MASS-ZWDOT) * ( lY-MQDOT) -ZQDOT*MWDOT 
A11V=( ( IY-MQDOT)*ZW+ZQDOT*MW)/DV 
A12V= ( ( I Y-MQDOT ) * ( ZQ+MASS ) +ZQDOT*MQ ) /DV 
A13V=- ( ZG-ZB ) * (MASS*XG+ZQDOT) *WEIGHT/DV 
A21V=(MWDOT*ZW+(MASS-ZWDOT)*MW)/DV 
A22V= (MWDOT* ( ZQ+MASS ) + (MASS-ZWDOT ) *MQ ) /DV 
A23V=- ( ZG-ZB) * (MASS-ZWDOT) *WEIGHT/DV 
B1 1V= ( ( I Y-MQDOT ) *  ZDS+ZQDOT*MDS ) /DV 
B1 2V= ( ( I Y-MQDOT ) *  ZDB+ZQDOT*MDB ) /DV 
B21V= (MWDOT* ZDS+( MASS-ZWDOT ) *MDS ) /DV 
B22V= ( MWDOT*  ZDB+ ( MASS-ZWDOT ) *MDB ) /DV 
BIV  =B11V-B12V 
B2V  =B21V-B22V 

EPS  =l,D-5 
ILMAX=1500 

LOOP  OVER  TV 

DO  1  1=1, ITV 

WRITE  (*,2001)  I, ITV 

TV=TVMIN+ (!-!)*( TVMAX-TVMIN ) / ( ITV- 1 ) 
OMEGA V= ( 1 0 . 0  *U ) / ( TV*L ) 

AD1V=1.75*0MEGAV 
AD2V=2 . 15*OMEGAV**2 
AD3V=OMEGAV**3 
A2=B1V*U*U 
A3=B2V*U*U 

D1=-AD1V- ( A11V+A22V) *U 
B1=-B2V*U*U 

B2=(B1V*A22V-B2V*A12V)*U**3 

B3=(B2V*A11V-B1V*A21V)*U**3 

D2=AD2V+A2  3V+ ( A12V* A2 IV-Al IV* A2  2V ) *U*  *  2 

C1=(B2V*A11V-B1V*A21V)*U**3 

C2=(A23V*B1V-A13V*B2V)*U**2 

D3=AD3V+ ( A1 3V*A2 IV-Al IV* A2  3V ) *U 

K2V=(A3*B1*D3+C1*B3*D1-D2*C1*A3) 

K2V=K2V/(A3*B1*C2+C1*B3*A2-C1*A3*B2) 

K1V=(D3-C2*K2V)/C1 

K3V=(D1-A2*K2V)/A3 

D333=(A13V*A21V-A11V*A23V)*U 

XAAA=CBRT(-D333) 
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D334=(D3*B2*C1*A3+B3*C1*A2*A3-B3*C1*D1*C2-D1*C1*C2*A3) 

D335=B3*C1*A2+B2*C1*A3-B1*C2*A3 

D336=D334/D335 

XBBB=CBRT(D336) 

ANALYTICAL  COMPUTATION 

IF  (ZG.NE.0.0)  TVCR1=(10.*U)/(XAAA*L) 

IF  (ZG.NE.0.0)  TVCR2=(10.*U)/(XAAA*L) 

IF  (ISOL.EQ.O)  GO  TO  22 
CXD2=AD3V* (AD1V*AD2V-AD3V) 

CXD1=- ( B2+A3*U) *K1V* ( AD1V*AD2V-AD3V) +AD3V*K1V* 

&  (A2*AD1V+B2+A3*U)-AD1V*AD1V*K1V*(-C2+C1*U) 

CXD0=-(B2+A3*U)*K1V*K1V*(AD1V*A2+B2+A3*U) 
DET=CXD1*CXD1-4.0*CXD2*CXD0 
IF  (DET.LT.0.0)  GO  TO  1 
XD1= ( -CXDl+DSQRT ( DET ) ) / ( 2 . 0*CXD2 ) 
XD2=(-CXD1-DSQRT(DET) )/(2.0*CXD2 ) 

IF  (XDI.NE.0.0) 

&  VALl=AD3V+( (B2V*A12V-B1V*A22V-B2V)*K1V*U**3)/XD1 

IF  (XD2.NE.0.0) 

&  VAL2=AD3V+( (B2V*A12V-B1V*A22V-B2V) *K1V*U**3 ) /XD2 

GO  TO  23 

NUMERICAL  COMPUTATION 
LOOP  OVER  XD 

22  DO  2  J=1,IXD 

XD=XDMIN+ ( J- 1 ) * ( XDMAX-XDMIN ) / ( IXD- 1 ) 

THETA=0.0D0 
CT=DCOS( THETA) 

ST=DSIN(THETA) 

W=0.0D0 
A(l, 1)=0.0D0 
A(1,2)=0.0D0 
A(1,3)=1.0D0 
A(1,4)=0.0D0 

A( 2 , 1 ) =B1V*U*U*K1V+A13V*CT 
A(2,2)=B1V*U*U*K2V+A11V*U 
A( 2 , 3 ) =B1V*U*U*K3V+A12V*U 
A(2,4)=-B1V*U*U*K1V/XD 
A(3, 1)=B2V*U*U*K1V+A23V*CT 
A ( 3 , 2 ) =B2V*U*U*K2V+A2 1V*U 
A ( 3 , 3 ) =B2V*U*U*K3V+A22V*U 
A( 3 , 4 ) =-B2V*U*U*KlV/XD 
A(4, 1)=-U*CT-W*ST 
A(4,2)=CT 
A(4,3)=0.0D0 
A(4,4)=0.0D0 

COMPUTE  EIGENVALUES 
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CALL  RG(4,4,A,WR,WI,0,ZZZ, IV1,FV1, lERR) 
CALL  DSTABL{ DECS, WR,WI, FREQ) 

IF  (J.GT.l)  GO  TO  10 

DEOSOO=DEOS 

XDOO  =XD 

LL=0 

GO  TO  2 

DEOSNN=DEOS 

XDNN  =XD 

PR=DEOSNN*DEOSOO 

IF  (PR.GT.O.DO)  GO  TO  3 

LL=LL+1 

IF  (LL.GT.3)  STOP  1000 
IL=0 

XDO=XDOO 

XDN=XDNN 

DEOSO=DEOSOO 

DEOSN=DEOSNN 

XDL=XDO 

XDR=XDN 

DEOSL=DEOSO 

DEOSR=DEOSN 

XD=(XDL+XDR)/2 .DO 

A(l, 1)=0.0D0 

A(1,2)=0.0D0 

A(1,3)=1.0D0 

A(1,4)=0.0D0 

A(2, 1)=B1V*U*U*K1V+A13V*CT 
A(  2 , 2 ) =B1V*U*U*K2V+A1 1V*U 
A( 2 , 3 ) =B1V*U*U*K3V+A12V*U 
A(  2 , 4 ) =-BlV*U*U*KlV/XD 
A( 3 , 1 ) =B2V*U*U*K1V+A23V*CT 
A( 3 , 2 ) =B2V*U*U*K2V+A2 1V*U 
A( 3 , 3 ) =B2V*U*U*K3V+A22V*U 
A( 3 , 4 ) =-B2V*U*U*K1V/XD 
A(4, 1)=-U*CT-W*ST 
A(4,2)=CT 
A(4, 3)=0.0D0 
A(4,4)=0.0D0 

CALL  RG(4,4,A,WR,WI,0,ZZZ, IV1,FV1, lERR) 
CALL  DSTABL(DE0S,WR,WI,FREQ) 

DEOSM=DEOS 

XDM=XD 

PRL=DE0SL*DE0SM 
PRR=DE0SR*DE0SM 
IF  (PRL.GT.O.DO)  GO  TO  5 


XDN=XDM 

DEOSO=DEOSL 

DEOSN=DEOSM 

IL=IL+1 

IF  ( IL.GT. ILMAX)  STOP  3100 
DIF=DABS(XDL-XDM) 

IF  (DIF.GT.EPS)  GO  TO  6 

XD=XDM 

GO  TO  4 

5  IF  (PRR.GT.O.DO)  STOP  3200 

XDO=XDM 
XDN=XDR 
DEOSO=DEOSM 
DEOSN=DEOSR 
IL=IL+1 

IF  (IL.GT. ILMAX)  STOP  3100 
DIF=DABS(XDM-XDR) 

IF  (DIF.GT.EPS)  GO  TO  6 
XD=XDM 

4  LLL=10+LL 

WRITE  (LLL,*)  XD/L,TV 
3  XDOO=XDNN 

DEOSOO=DEOSNN 
2  CONTINUE 
GO  TO  1 

23  IF  (VALI.GT.0.0)  WRITE  (11,*)  XD1/L,TV 
IF  (VAL2.GT.0.0)  WRITE  (12,*)  XD2/L,TV 
1  CONTINUE 

IF  (ZG.NE.0.0)  WRITE  (10,*)  XDMIN/L, TVCRl 
IF  (ZG.NE.0.0)  WRITE  (10,*)  XDMAX/L, TVCRl 

1001  FORMAT  ('  ENTER  MIN,  MAX,  AND  INCREMENTS  OF  TV) 

1002  FORMAT  ('  ENTER  MIN,  MAX,  AND  INCREMENTS  OF  XD ' ) 

1003  FORMAT  ('  ENTER  U  AND  ZG’) 

1004  FORMAT  ('  ENTER  0  ;  NUMERICAL',/, 

&  '  1  :  ANALYTICAL ’ ) 

2001  FORMAT  (215) 

END 

SUBROUTINE  DSTABL ( DEOS , WR, WI , OMEGA ) 

IMPLICIT  DOUBLE  PRECISION  (A-H,0-Z) 

DIMENSION  WR(4),WI(4) 

DEOS=-l .OD+20 
DO  1  1=1,4 

IF  (WR(I) .LT.DEOS)  GO  TO  1 
DEOS=WR( I) 

IJ=I 

1  CONTINUE 
OMEGA=WI( IJ) 

OMEGA=DABS ( OMEGA ) 

RETURN 
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END 


FUNCTION  CBRT(A) 

IF  (A.GT.O.O)  CBRT=  A  **( 
IF  (A.LE.0.0)  CBRT=-(-A)**( 
RETURN 
END 


./3.) 
./3.  ) 
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APPENDIX  C 


PROGRAM  VERT_STEADY . FOR 

COMPUTATION  OF  STEADY  STATE  SOLUTIONS  IN  THE  VERTICAL 

PLANE 

(CHAPTER  III,  PARAGRAPH  F) 

REAL  K1V,K2V,K3V,L,MQD0T,MQ,MW,MWD0T,MDS,MDB,MASS, lY 
C 

WEIGHT=12000.0 
lY  =  9450.0 

L  =  17.425 

RHO  =  1.94 

G  =  32.2 

XG  =  0.0 

ZB  =  0.0 

MASS  =WEIGHT/G 
BOY  =WEIGHT 

C 

ZQDOT  =-6.810E-03*0.5*RHO*L**4 
ZWDOT  =-2.430E-01*0.5*RHO*L**3 
ZQ  =-1.350E-01*0.5*RHO*L**3 
ZW  =-3.020E-01*0.5*RHO*L**2 
ZDS  =-2 .270E-02*0.5*RHO*L**2 
ZDB  =-2.270E-02*0.5*RHO*L**2 
C 

MQDOT  =-l . 680E-02*0.5*RHO*L**5 
MWDOT  =-6.810E-02*0.5*RHO*L**4 
MQ  =-6.860E-02*0.5*RHO*L**4 
MW  =  9 .860E-02*0.5*RHO*L**3 
MDS  =-1.113E-02*0.5*RHO*L**3 
MDB  =  1 . 113E-02*0.5*RHO*L**3 
C 

OPEN  (11,FILE='THETA1.RES' , STATUS= ' NEW ’ ) 

OPEN  (12,FILE= ’THETA2.RES ■ ,STATUS=’NEW' ) 

OPEN  ( 1 3, FILE= 'THETAS. RES' , STATUS= ' NEW ' ) 

OPEN  (14,FILE='THETA4.RES' , STATUS= ' NEW ' ) 

OPEN  (21,FILE='DELTA1.RES' , STATUS= ' NEW ' ) 

OPEN  ( 22 , FILE= ' DELTA2 . RES ' , STATUS = ' NEW ' ) 

C 

SAT  =0.4 
SATP=  SAT 
SATM=-SAT 
PI  =4.0*ATAN(1.0) 

C 
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WRITE  (*,1001) 

READ  ( * , * )  IVAR 

GO  TO  (10,20,30),  IVAR 

10  WRITE  (*,1002) 

READ  (*,*)  UMIN,UMAX,IU 

INCR=IU 

WRITE  (*,1003) 

READ  ( * , *  )  ZG 

WRITE  (*,1006) 

READ  ( * , * )  TV 

GO  TO  15 

20  WRITE  (*,1004) 

READ  (*,*)  ZGMIN,ZGMAX,IZG 

INCR=IZG 

WRITE  (*,1005) 

READ  ( * , *  )  U 

WRITE  (*,1006) 

READ  ( * , * )  TV 

GO  TO  15 

30  WRITE  (*,1007) 

READ  (*,*)  TVMIN,TVMAX, ITV 

INCR=ITV 

WRITE  (*,1003) 

READ  ( * , *  )  ZG 

WRITE  (*,1005) 

READ  ( * , *  )  U 

C 

15  DO  1  I=1,INCR 

IF  (IVAR.EQ.l)  U  =UMIN  +(UMAX  -UMIN  ) * ( I- 1 ) / ( INCR- 1 ) 
IF  (IVAR.EQ.2)  ZG=ZGMIN+(ZGMAX-ZGMIN)*(I-1)/(INCR-1) 
IF  (IVAR.EQ,3)  TV=TVMIN+(TVMAX-TVMIN)*(I-1)/(INCR-1) 
DV  =(MASS-ZWDOT)* ( lY-MQDOT) -ZQDOT*MWDOT 
A11V=( (IY-MQDOT)*ZW+ZQDOT*MW)/DV 
A1 2 V= ( ( I Y-MQDOT ) * ( ZQ+MASS ) +ZQDOT*MQ ) /DV 
A13V=- ( ZG-ZB ) * (MASS*XG+ZQDOT) *WEIGHT/DV 
A2 1 V= ( MWDOT  *  Z W+ ( MAS  S - Z WDOT ) *MW ) /DV 
A22V= (MWDOT* ( ZQ+MASS ) + (MASS-ZWDOT ) *MQ ) /DV 
A23V=- ( ZG-ZB) * (MASS-ZWDOT) *WEIGHT/DV 
B1 1V= ( ( I Y-MQDOT ) *  ZDS+ZQDOT*MDS ) /DV 
B1 2V= ( ( I Y-MQDOT ) *  ZDB+ZQDOT*MDB ) /DV 
B21V= (MWDOT* ZDS+ (MASS-ZWDOT) *MDS)/DV 
B22V=(MWDOT*ZDB+ (MASS-ZWDOT) *MDB)/DV 
BIV  =B11V-B12V 
B2V  =B21V-B22V 
C 

OMEGAV=(10.0*U)/(TV*L) 

AD1V=1.75*0MEGAV 
AD2V=2 . 15*OMEGAV**2 
AD3V=OMEGAV**3 
A2=B1V*U*U 
A3=B2V*U*U 
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D1=-AD1V- ( A11V+A22V) *U 
B1=-B2V*U*U 

B2=(B1V*A22V-B2V*A12V)*U**3 
B3=(B2V*A11V-B1V*A21V) *U**3 
D2=AD2V+A23V+(A12V*A21V-A11V*A22V)*U**2 
C1=(B2V*A11V-B1V*A21V)*U**3 
C2=(A23V*B1V-A13V*B2V)*U**2 
D3=AD3V+(A13V*A21V-A11V*A23V)*U 
K2V=(A3*B1*D3+C1*B3*D1-D2*C1*A3) 
K2V=K2V/(A3*B1*C2+C1*B3*A2-C1*A3*B2) 
K1V=(D3-C2*K2V)/C1 
K3V=(D1-A2*K2V)/A3 
C 

IF  (IVAR.EQ.l)  OUT=U 
IF  (IVAR.EQ.2)  OUT=ZG 
IF  (IVAR.EQ.3)  OUT=TV 
D3P=(A13V*A21V-A11V*A23V)*U 
XAAA=CBRT(-D3P) 

TVCR= ( 1 0 . *U ) / ( XAAA*L ) 

IF  (TV.LT.TVCR)  GO  TO  1 
C 

CALL  S0LSET(INUM,THS0LS,K1V,C1,AD3V,SSTH) 

ICHECK=0 

DO  2  III=1,INUM 

THCH=2 . 0* (THSOLS-0 . 5*PI ) 

CHECK=S IN  <  THCH ) * ( D3-AD3V ) /Cl 
IF  (ABS (CHECK) .GT.SATP)  GO  TO  2 
WRITE  (13,*)  OUT,  THCH*180.0/PI 
WRITE  (14,*)  OUT, -THCH*180. 0/PI 
WRITE  (21,*)  OUT,  ABS(CHECK)*180.0/PI 
ICHECK=1 

2  CONTINUE 

IF  ( ICHECK.EQ.O)  GO  TO  3 
GO  TO  1 
C 

3  STHETA=SATP*C1/(D3-AD3V) 

SSTH=ASIN(STHETA) 

THETA1=  SSTH*180.0/PI 
THETA2=-SSTH*180 .0/PI 
WRITE  (11,*)  OUT,THETAl 
WRITE  (12,*)  OUT,THETA2 

WRITE  (22  *)  OUT,  SATP*180 . 0/PI 
1  CONTINUE 
STOP 

1001  FORMAT  ('  ENTER  1  ;  U  VARIATION’,/, 

&  ’  2  :  ZG  VARIATION’ ,/, 

&  '  3  :  TV  VARIATION’ ) 

1002  FORMAT  (’  ENTER  MIN,  MAX,  AND  INCREMENTS  IN  U’) 

1003  FORMAT  ('  ENTER  ZG ’ ) 

1004  FORMAT  (’  ENTER  MIN,  MAX,  AND  INCREMENTS  IN  ZG’) 

1005  FORMAT  (’  ENTER  U’) 
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1006  FORMAT  ('  ENTER  TV') 

1007  FORMAT  (’  ENTER  MIN,  MAX,  AND  INCREMENTS  IN  TV) 

END 

C 

FUNCTION  CBRT(A) 

IF  (A.GT.0.0)  CBRT=  A  **(l./3.) 

IF  (A.LE.0.0)  CBRT=-(-A)**(l./3. ) 

RETURN 

END 

C 

SUBROUTINE  SOLSET( L, ANS , KIV, Cl , AD3V, SSTH ) 

REAL  KIV 

DIMENSION  VF(1,2) 

C 

PI=4.0*ATAN(1.0) 

C 

C  FIND  FIRST  ESTIMATE  OF  THE  SOLUTIONS 

L=0 

VMIN=  0.0 
VMAX=+90.0 
IV=100 

VA=VMIN*PI/180.0 

VAO=VA 

VO=THETEQ ( 1 , VA , K 1 V , C 1 , AD3V ) 

DO  10  1=2, IV 

VA=VMIN+ ( VMAX-VMIN ) * ( I- 1 ) / ( IV- 1 ) 

VA=VA*PI/180.0 

VAN=VA 

VN=THETEQ( 1,VA,K1V,C1,AD3V) 

VP=VO*VN 

IF  (VP.GE.0.0)  GO  TO  11 
L=L+1 

VF(L,l)=VAO 
VF(L,2)=VAN 
GO  TO  12 

11  VO=VN 
VAO=VAN 

10  CONTINUE 
C 

C  EXACT  COMPUTATION  OF  SOLUTIONS  VIA  NEWTON’S  METHOD 

12  E=l.E-5 
IEND=500 

DO  20  J=1,L 

X=(VF( J, 1)+VF( J,2) )/2 .0 
F=THETEQ( 1,X,K1V,C1,AD3V) 

FDER=THETEQ ( 2 , X , K1 V , Cl , AD3V ) 

DO  30  K=1,IEND 

IF  (FDER.EQ.0.0)  STOP  1001 

DX=F/FDER 

X1=X-DX 

F=THETEQ( 1, XI, KIV, Cl, AD3V ) 
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FDER=THETEQ (2,X1,K1V,C1, AD3V ) 

IF  (F.EQ.O. )  GO  TO  35 
A=ABS(X1-X) 

IF  (A-E)  35,35,40 
40  X=X1 
30  CONTINUE 
GO  TO  20 
35  ANS=X1 
20  CONTINUE 
RETURN 
END 

FUNCTION  THETEQ( K, THETA, KIV, Cl, AD3V) 

REAL  KIV 

GO  TO  (10,20),  K 

10  THETEQ=K1V*C1*THETA+(AD3V-K1V*C1)*C0S (THETA) 
GO  TO  50 

20  THETEQ=K1V*C1-(AD3V-K1V*C1  )*SIN(THET?.) 

50  RETURN 
END 
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